Compare commits

...

8 Commits
master ... rtos

18 changed files with 709 additions and 1673 deletions
Split View
  1. +2
    -2
      Makefile
  2. +16
    -102
      at91_init0.s
  3. +98
    -9
      at91_init1.c
  4. +70
    -0
      include/rtos/context.h
  5. +22
    -0
      include/rtos/semaphore.h
  6. +19
    -0
      include/rtos/spinlock.h
  7. +26
    -50
      main.c
  8. +0
    -92
      src/at91_adc.c
  9. +52
    -10
      src/at91_exceptions.c
  10. +0
    -79
      src/at91_pitc.c
  11. +0
    -34
      src/at91_rttc_test.c
  12. +0
    -227
      src/at91_tc1.c
  13. +0
    -261
      src/at91_twi.c
  14. +0
    -552
      src/at91_udp.c
  15. +308
    -0
      src/rtos/context.c
  16. +56
    -0
      src/rtos/semaphore.c
  17. +40
    -0
      src/rtos/spinlock.c
  18. +0
    -255
      src/telemetrie.c

+ 2
- 2
Makefile View File

@@ -10,8 +10,8 @@ SIZE = $(TOOLCHAIN)/bin/arm-elf-size
OBJCOPY = $(TOOLCHAIN)/bin/arm-elf-objcopy
OBJDUMP = $(TOOLCHAIN)/bin/arm-elf-objdump

INCDIRS = include $(TOOLCHAIN)/lib/gcc/arm-elf/4.1.1/include $(TOOLCHAIN)/arm-elf/include
LIBDIRS = $(TOOLCHAIN)/arm-elf/lib $(TOOLCHAIN)/lib/gcc/arm-elf/4.1.1
INCDIRS = include $(TOOLCHAIN)/lib/gcc/arm-elf/4.3.3/include $(TOOLCHAIN)/arm-elf/include
LIBDIRS = $(TOOLCHAIN)/arm-elf/lib $(TOOLCHAIN)/lib/gcc/arm-elf/4.3.3

# ------



+ 16
- 102
at91_init0.s View File

@@ -22,13 +22,14 @@
.equ AT91C_BASE_AIC, (0xFFFFF000)

.equ IRQ_Stack_Size, (3 * 8 * 4)
.equ FIQ_Stack_Size, (3 * 8 * 4)
.equ FIQ_Stack_Size, (0 * 8 * 4)
.equ ABT_Stack_Size, 192

.equ ARM_MODE_FIQ, 0x11
.equ ARM_MODE_IRQ, 0x12
.equ ARM_MODE_SVC, 0x13
.equ ARM_MODE_ABT, 0x17
.equ ARM_MODE_SYS, 0x1F

.equ I_BIT, 0x80
.equ F_BIT, 0x40
@@ -46,15 +47,19 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */
irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */
fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */

.extern SWI_Handler
.extern ABT_Handler
.extern IRQ_Handler

/* 0x80000000 will result in Prefetch Abort */
.word InitReset
.word 0x80000000
.word SWI_Handler
.word ABT_Handler
.word ABT_Handler
.word 0x80000000
.word ABT_Handler_Entry
.word ABT_Handler_Entry
.word 0x80000000
.word IRQ_Handler_Entry
.word FIQ_Handler_Entry
.word IRQ_Handler
.word FIQ_Handler

.endfunc

@@ -88,7 +93,7 @@ InitReset:
sub r0, r0, #ABT_Stack_Size

/* Setup Supervisor Mode Stack (IRQ & NMI enabled) */
msr CPSR_c, #ARM_MODE_SVC
msr CPSR_c, #ARM_MODE_SYS
mov sp, r0

/* Relocate .data section */
@@ -121,45 +126,9 @@ LoopZI: cmp r1, r2
exit: b .
.endfunc

.global ABT_Handler_Entry
.func ABT_Handler_Entry
ABT_Handler_Entry:
/* disable interrupts (F_BIT not set on entry) */
msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT

/* store all registers */
stmfd sp!, { r0-r12 }

/* saved cpsr (from aborted mode) */
mrs r0, SPSR

/* address of abort (pc) */
mov r3, lr

/* enter previous mode and get lr(r14), sp(r13) */
/* TODO: interrupts might be enabled? */
/* TODO: thumb mode enabled? */
msr CPSR_c, r0
mov r1, sp
mov r2, lr

/* return to abort mode */
msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT

/* store remaining registers (r1-r3 == r13-r15) */
stmfd sp!, { r1-r3 }
mov r1, sp

/* execute C Handler (cpsr, registers) */
ldr r5, =at91_abt_handler
mov lr, pc
bx r5
b .
.endfunc

.global FIQ_Handler_Entry
.func FIQ_Handler_Entry
FIQ_Handler_Entry:
.global FIQ_Handler
.func FIQ_Handler
FIQ_Handler:
/* Save r0 to ARM_MODE_FIQ:r9 */
mov r9, r0

@@ -167,7 +136,7 @@ FIQ_Handler_Entry:
ldr r0, [r8, #AIC_FVR]

/* Switch to ARM_MODE_SVC and save registers there */
msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT
msr CPSR_c, #ARM_MODE_SYS | I_BIT | F_BIT
stmfd sp!, { r1-r3, r12, lr }

/* execute FIQ in SVC_MODE */
@@ -184,58 +153,3 @@ FIQ_Handler_Entry:
/* restore PC using the LR_fiq directly */
subs pc, lr, #4
.endfunc

.global IRQ_Handler_Entry
.func IRQ_Handler_Entry
IRQ_Handler_Entry:
/* Manage Exception Entry */
/* Adjust and save LR_irq in IRQ stack */
sub lr, lr, #4
stmfd sp!, { lr }

/* Save SPSR need to be saved for nested interrupt */
mrs r14, SPSR
stmfd sp!, { r14 }

/* Save and r0 in IRQ stack */
stmfd sp!, { r0 }

/* Write in the IVR to support Protect Mode */
/* No effect in Normal Mode */
/* De-assert the NIRQ and clear the source in Protect Mode */
ldr r14, =AT91C_BASE_AIC
ldr r0, [r14, #AIC_IVR]
str r14, [r14, #AIC_IVR]

/* Enable Interrupt and Switch in Supervisor Mode */
msr CPSR_c, #ARM_MODE_SVC

/* Save scratch/used registers and LR in User Stack */
stmfd sp!, { r1-r3, r12, r14 }

/* Branch to the routine pointed by the AIC_IVR */
mov r14, pc
bx r0

/* Restore scratch/used registers and LR from User Stack */
ldmia sp!, { r1-r3, r12, r14 }

/* Disable Interrupt and switch back in IRQ mode */
msr CPSR_c, #ARM_MODE_IRQ | I_BIT

/* Mark the End of Interrupt on the AIC */
ldr r14, =AT91C_BASE_AIC
str r14, [r14, #AIC_EOICR]

/* Restore SPSR_irq and r0 from IRQ stack */
ldmia sp!, { r0 }

/* Restore SPSR_irq and r0 from IRQ stack */
ldmia sp!, { r14 }
msr SPSR_cxsf, r14

/* Restore adjusted LR_irq from IRQ stack directly in the PC */
ldmia sp!, { pc }^

.endfunc
.end

+ 98
- 9
at91_init1.c View File

@@ -31,33 +31,33 @@ void at91_init1(void)
{
/* disable watchdog */
*AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS;
/* enable user reset */
*AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN;

/* Set Flash Waitstates */
*AT91C_MC_FMR = AT91C_MC_FWS_1FWS;
/*
/*
* Enable main oscillator (MAINCK)
* startup time: 8*6/32768 -> 1.46ms
*/
AT91S_PMC *pmc = AT91C_BASE_PMC;
pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) |
pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) |
AT91C_CKGR_MOSCEN;
while (!(pmc->PMC_SR & AT91C_PMC_MOSCS));

/*
/*
* PLLCK = 18.432MHz / 24 * 125 = 96MHz -> div:24, mul:124
* startup time: 32/32768 -> 976us
*/
pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) |
(AT91C_CKGR_MUL & (124<<16)) |
*/
pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) |
(AT91C_CKGR_MUL & (124<<16)) |
(AT91C_CKGR_PLLCOUNT & (32<<8)) ;
while (!(pmc->PMC_SR & AT91C_PMC_LOCK));

/* MCK = PLLCK / 2 = 48MHz */
pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK |
pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK |
AT91C_PMC_PRES_CLK_2;
while (!(pmc->PMC_SR & AT91C_PMC_MCKRDY));

@@ -79,3 +79,92 @@ void at91_init1(void)
}
aic->AIC_SPU = (uint32_t)empty_isr;
}

/* TODO: make it static */
uint32_t nested_count = 0;

__attribute__((naked)) void IRQ_Handler(void)
{
asm volatile (
".equ AIC_IVR_OFF, (256) \n\t"
".equ AIC_EOICR_OFF, (304) \n\t"
".equ AT91C_BASE_AIC, (0xFFFFF000) \n\t"
".equ ARM_MODE_IRQ, 0x12 \n\t"
".equ ARM_MODE_SYS, 0x1F \n\t"
".equ I_BIT, 0x80 \n\t"
".equ F_BIT, 0x40 \n\t"

/* Manage Exception Entry */
/* Save LR_irq to IRQ stack */
"sub lr, lr, #4 \n\t"
"stmdb sp!, { lr } \n\t"

/* Save r0 and SPSR to IRQ stack */
"mrs r14, SPSR \n\t"
"stmdb sp!, { r0, r14 } \n\t"

/* count nested interrupts */
"ldr r14, =nested_count \n\t"
"ldr r0, [r14] \n\t"
"add r0, r0, #1 \n\t"
"str r0, [r14] \n\t"

/* De-assert the NIRQ and clear the source in Protect Mode */
"ldr r14, =AT91C_BASE_AIC \n\t"
"ldr r0, [r14, #AIC_IVR_OFF] \n\t"
"str r14, [r14, #AIC_IVR_OFF] \n\t"

/* Enable Interrupt and Switch in Supervisor Mode */
"msr CPSR_c, #ARM_MODE_SYS \n\t"

/* Save scratch/used registers and LR in User Stack */
"stmdb sp!, { r1-r3, r12, r14 } \n\t"

/* Branch to the routine pointed by the AIC_IVR */
"mov r14, pc \n\t"
"bx r0 \n\t"

/* Restore scratch/used registers and LR from User Stack */
"ldmia sp!, { r1-r3, r12, r14 } \n\t"

/* Disable Interrupt and switch back in IRQ mode */
"msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t"

/* Mark the End of Interrupt on the AIC */
"ldr r14, =AT91C_BASE_AIC \n\t"
"str r14, [r14, #AIC_EOICR_OFF] \n\t"

/* count nested interrupts */
"ldr r14, =nested_count \n\t"
"ldr r0, [r14] \n\t"
"subs r0, r0, #1 \n\t"
"str r0, [r14] \n\t"
"beq irq_ctx_save \n\t"

/* Restore r0 and SPSR_irq from IRQ stack */
"ldmia sp!, { r0, r14 } \n\t"
"msr SPSR_cxsf, r14 \n\t"

/* Restore adjusted LR_irq from IRQ stack */
"ldmia sp!, { pc }^ \n\t"

/* save the userspace context to current_context */
"irq_ctx_save: \n\t"

/* get top of struct register_context */
"ldr r0, =current_context \n\t"
"ldr r0, [r0] \n\t"
"add r0, r0, #68 \n\t"

/* save usermode cpsr & r2-14 */
"mrs r1, spsr \n\t"
"stmdb r0, {r1-r14}^ \n\t"
"nop \n\t"
"sub r0, r0, #56 \n\t"

/* save r0-1 and svc_lr (= pc) */
"ldmia sp!, {r1, r2} \n\t"
"stmdb r0!, {r1, r2, r14} \n\t"

);
}

+ 70
- 0
include/rtos/context.h View File

@@ -0,0 +1,70 @@
#ifndef _CONTEXT_H_
#define _CONTEXT_H_

#include <stdint.h>
#include "rtos/spinlock.h"

struct register_context {
uint32_t r0;
uint32_t r1;
uint32_t pc;
uint32_t cpsr;
uint32_t r2;
uint32_t r3;
uint32_t r4;
uint32_t r5;
uint32_t r6;
uint32_t r7;
uint32_t r8;
uint32_t r9;
uint32_t r10;
uint32_t r11;
uint32_t r12;
uint32_t sp;
uint32_t r14;
};

enum ctx_state {
CONTEXT_NULL = 0,
CONTEXT_RUNNING = 1,
CONTEXT_READY = 2,
CONTEXT_INTERRUPTED = 3,
CONTEXT_SLEEP = 4,
CONTEXT_SLEEP_QUEUE = 5,
};

struct context {
/* regs *MUST* be first in struct! */
struct register_context regs;
void *stack;
uint8_t state;
uint8_t priority;

/* next pointers for run & sleep queues */
struct context *run_queue;
struct context *sleep_queue;
};

extern struct context *current_context;

void isr_context_yield(void);
void context_yield(void);

uint8_t isr_context_wait(struct spinlock *lock);
uint8_t context_wait(struct spinlock *lock);
uint8_t context_wait_queue(struct spinlock *lock, struct context **queue);
uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue);

void isr_context_signal(struct context *c);
void context_signal(struct context *c);
uint32_t context_signal_queue(struct context **queue);

void isr_context_interrupt(struct context *c);
void context_interrupt(struct context *c);
uint32_t context_interrupt_queue(struct context *c, struct context **queue);

struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg);

void init_context(void);

#endif /* _CONTEXT_H_ */

+ 22
- 0
include/rtos/semaphore.h View File

@@ -0,0 +1,22 @@
#ifndef _SEMSPHORE_H_
#define _SEMSPHORE_H_

#include <stdint.h>

#include "rtos/context.h"
#include "rtos/spinlock.h"

struct semaphore {
struct context *sleep_queue;
struct spinlock lock;
int32_t count;
};

uint8_t sem_wait(struct semaphore *sem);
void sem_post(struct semaphore *sem);

void sem_interrupt(struct semaphore *sem, struct context *c);
int32_t sem_get_count(struct semaphore *sem);
void sem_init(struct semaphore *sem, int32_t count);

#endif /* _SEMSPHORE_H_ */

+ 19
- 0
include/rtos/spinlock.h View File

@@ -0,0 +1,19 @@
#ifndef _SPINLOCK_H_
#define _SPINLOCK_H_

#include <stdint.h>

struct spinlock {
uint8_t locked;
uint8_t priority_unlocked;
};

void isr_spinlock_lock(struct spinlock *lock);
void spinlock_lock(struct spinlock *lock);

void isr_spinlock_unlock(struct spinlock *lock);
void spinlock_unlock(struct spinlock *lock);

void spinlock_init(struct spinlock *lock);

#endif /* _SPINLOCK_H_ */

+ 26
- 50
main.c View File

@@ -16,46 +16,34 @@
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_sysc.h"
#include "at91_dbgu.h"
#include "at91_pitc.h"
#include "at91_tests.h"
#include "at91_udp.h"
#include "at91_pio.h"
#include "at91_twi.h"

#include "at91_tc1.h"
#include "memalloc.h"
#include "rtos/context.h"
#include "rtos/semaphore.h"

#include "board.h"
#include <stdio.h>
#include <string.h>
static struct semaphore sem;

static uint32_t pitc_test(struct pitc_timer *timer)
void testisr(uint32_t status, uint32_t input)
{
static uint32_t i;
*AT91C_PIOA_SODR = i;
i = i ^ LED_GREEN;
*AT91C_PIOA_CODR = i;
/*
struct rc_values rc;
uint32_t count = rcontrol_getvalues(&rc);

printf("%ld channels: ", count);
uint32_t j;
for (j = 0; j < count; j++)
printf("%+5d ", rc.chan[j]);

printf("\r");
*/
return PITC_RESTART_TIMER;
printf("testisr: sem_post()\n\r");
sem_post(&sem);
}

static struct pitc_timer pitc_test_timer = {
.interval = 10,
.func = &pitc_test,
};
void testfunc(void *p)
{
while (1) {
printf("testfunc(%ld): sem_wait()\n\r", (uint32_t)p);
sem_wait(&sem);
}
}

int main(void)
{
@@ -63,33 +51,21 @@ int main(void)
*AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
*AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;

/* needed for dbgu */
at91_sysc_init();

at91_dbgu_init();
at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");

/* triggers pinchange-isrs */
at91_pio_init();

/* timer */
at91_pitc_init();
at91_rttc_test_init();
at91_tc1_init();
struct context *text_ctx1 = create_ctx(512, 0x80, testfunc, (void *)1);
printf("test_ctx(1)=%p\n\r", text_ctx1);

/* adc, need timer */
at91_adc_test_init();
struct context *text_ctx2 = create_ctx(512, 0x80, testfunc, (void *)2);
printf("test_ctx(2)=%p\n\r", text_ctx2);

/* twi */
at91_twi_init();
at91_twi_test();
sem_init(&sem, 0);

/* usb */
at91_udp_init();

printf("static alloc: %5ld bytes\n\r", static_alloc_used());

pitc_schedule_timer(&pitc_test_timer);

while (1);
init_context();
}

PIO_PINCHANGE_ISR(TAST1, testisr);

+ 0
- 92
src/at91_adc.c View File

@@ -1,92 +0,0 @@
/***************************************************************************
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; version 2 of the License *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include <stdint.h>
#include <stdio.h>

#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_pitc.h"

static uint16_t adc_result[4];

static void at91_adc_isr(void)
{
AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
pdc->PDC_RPR = (uint32_t) &adc_result;
pdc->PDC_RCR = ARRAY_SIZE(adc_result);
pdc->PDC_PTCR = AT91C_PDC_RXTEN;

/* clear interrupts */
AT91S_ADC *adc = AT91C_BASE_ADC;
uint32_t status = adc->ADC_SR;
status = status;
}

static uint32_t adc_trigger(struct pitc_timer *timer)
{
*AT91C_ADC_CR = AT91C_ADC_START;
return PITC_RESTART_TIMER;
}

static struct pitc_timer adc_timer = {
.interval = 100,
.func = &adc_trigger,
};

void at91_adc_test_init(void)
{
/* enable ADC clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_ADC);

/* ADC Software reset */
AT91S_ADC *adc = AT91C_BASE_ADC;
adc->ADC_CR = AT91C_ADC_SWRST;

/*
* ADC config: 10bit, no sleep
* 4.8MHz (48MHz / ((4 +1) * 2) = 4.8MHz)
* 96 cycles Startup ((11 +1) * 8 / 4.8MHz = 20us)
* 3 cycles SH ((2 +1) / 4.8MHz = 625ns)
* Conversion time per channel @5MHz ~2us
*/
adc->ADC_MR = AT91C_ADC_TRGEN_DIS |
AT91C_ADC_LOWRES_10_BIT |
AT91C_ADC_SLEEP_NORMAL_MODE |
(AT91C_ADC_PRESCAL & (4 << 8)) |
(AT91C_ADC_STARTUP & (11 << 16)) |
(AT91C_ADC_SHTIM & (2 << 24));

/* setup PDC */
AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
pdc->PDC_RPR = (uint32_t) &adc_result;
pdc->PDC_RCR = ARRAY_SIZE(adc_result);
pdc->PDC_PTCR = AT91C_PDC_RXTEN;

/* enable 4 channels, PDC Interrupt */
adc->ADC_CHER = 0xF0;
adc->ADC_IER = AT91C_ADC_ENDRX;

/* low priority, level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
aic->AIC_IECR = (1<<AT91C_ID_ADC);

pitc_schedule_timer(&adc_timer);
}

+ 52
- 10
src/at91_exceptions.c View File

@@ -39,7 +39,7 @@ static void dbgu_puts(const char *p)
static void dbgu_hexvar(const char *name, uint32_t var)
{
dbgu_puts(name);
char buf[11];
buf[0] = '0';
buf[1] = 'x';
@@ -47,7 +47,7 @@ static void dbgu_hexvar(const char *name, uint32_t var)
for (i = 9; i > 1; i--) {
buf[i] = (var & 0x0F);
buf[i] += (buf[i] < 0xa) ? '0' : ('a' - 0x0a);
var >>= 4;
var >>= 4;
}
buf[10] = '\0';
dbgu_puts(buf);
@@ -59,8 +59,8 @@ ARM7TDMI 32bit Undefined Address Instruction Fetch Abort
--- Registerdump (cpsr:0xa0000013 - SVC):
r0:0x00000002 r1:0x0000000c r2:0x00200038 r3:0x80000000
r4:0x0020fffc r5:0x00101788 r6:0x00000180 r7:0x41000000
r8:0x00008400 r9:0x00100004 r10:0x03000294 fp:0x0020fe84
r12:0x0020fe10 sp:0x0020fe70 lr:0x00102174 pc:0x80000004
r8:0x00008400 r9:0x00100004 sl:0x03000294 fp:0x0020fe84
ip:0x0020fe10 sp:0x0020fe70 lr:0x00102174 pc:0x80000004
--- Stackdump:
0x0020fe70: 0x0020fe7c 0x00101648 0x001015a8 0x0020feaf
0x0020fe80: 0x0020fec0 0x0020fe90 0x00101780 0x00101620
@@ -96,17 +96,17 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
dbgu_puts("32bit ");
break;
}
if (asr & AT91C_MC_UNDADD)
dbgu_puts("Undefined Address ");
else if (asr & AT91C_MC_MISADD)
dbgu_puts("Misaliged Address ");
switch (asr & AT91C_MC_ABTTYP) {
case AT91C_MC_ABTTYP_DATAR:
dbgu_puts("Data Read ");
break;
case AT91C_MC_ABTTYP_DATAW:
dbgu_puts("Data Write ");
break;
@@ -115,7 +115,7 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
dbgu_puts("Prefetch ");
break;
}
dbgu_hexvar("Abort\n(ASR:", asr);
dbgu_hexvar(" AASR:", *AT91C_MC_AASR);
dbgu_puts(")\n");
@@ -142,9 +142,9 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
dbgu_hexvar(" r7:", registers[10]);
dbgu_hexvar("\n r8:", registers[11]);
dbgu_hexvar(" r9:", registers[12]);
dbgu_hexvar(" r10:", registers[13]);
dbgu_hexvar(" sl:", registers[13]);
dbgu_hexvar(" fp:", registers[14]);
dbgu_hexvar("\nr12:", registers[15]);
dbgu_hexvar("\n ip:", registers[15]);
dbgu_hexvar(" sp:", registers[0]);
dbgu_hexvar(" lr:", registers[1]);
dbgu_hexvar(" pc:", registers[2]);
@@ -160,3 +160,45 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
}
dbgu_putchar('\n');
}


__attribute__((naked)) void ABT_Handler(void)
{
asm volatile (
".equ ARM_MODE_ABT, 0x17 \n\t"
".equ I_BIT, 0x80 \n\t"
".equ F_BIT, 0x40 \n\t"

/* disable interrupts (F_BIT not set on entry) */
"msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t"

/* store all registers */
"stmfd sp!, { r0-r12 } \n\t"

/* saved cpsr (from aborted mode) */
"mrs r0, SPSR \n\t"

/* address of abort (pc) */
"mov r3, lr \n\t"

/* enter previous mode and get lr(r14), sp(r13) */
/* TODO: interrupts might be enabled? */
/* TODO: thumb mode enabled? */
"msr CPSR_c, r0 \n\t"
"mov r1, sp \n\t"
"mov r2, lr \n\t"

/* return to abort mode */
"msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t"

/* store remaining registers (r1-r3 == r13-r15) */
"stmfd sp!, { r1-r3 } \n\t"
"mov r1, sp \n\t"

/* execute C Handler (cpsr, registers) */
"ldr r5, =at91_abt_handler \n\t"
"mov lr, pc \n\t"
"bx r5 \n\t"
"b . \n\t"
);
}

+ 0
- 79
src/at91_pitc.c View File

@@ -1,79 +0,0 @@
/***************************************************************************
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; version 2 of the License *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include "AT91SAM7S256.h"
#include "at91_pitc.h"
#include "at91_sysc.h"
#include "board.h"

/* PIV is 20bit -> min. 3Hz @48MHz MCK */
#define HZ_TO_PIV(HZ) (MCK / (16 * HZ))

static LIST_HEAD(timer_list);
volatile static uint32_t pitc_ticks;

void pitc_schedule_timer(struct pitc_timer *timer)
{
timer->nextrun = timer->interval + pitc_ticks;

struct pitc_timer *search;
list_for_each_entry(search, &timer_list, list)
if (search->nextrun > timer->nextrun)
break;

list_add_tail(&timer->list, &search->list);
}

static void pitc_isr(uint32_t status)
{
/* get Ticks and clear interrupt */
pitc_ticks += (*AT91C_PITC_PIVR & AT91C_PITC_PICNT) >> 20;

struct pitc_timer *search, *tmp;
list_for_each_entry_safe(search, tmp, &timer_list, list) {
/* if this entry not scheduled yet, abort search */
if (pitc_ticks < search->nextrun)
break;

/* remove from list */
list_del(&search->list);

/* exec handler */
if (search->func(search) == PITC_REMOVE_TIMER) {
/* one-shot timer, mark as completed */
search->nextrun = 0x00;
continue;
}
/* interval timer, reschedule it */
pitc_schedule_timer(search);
}
}

uint32_t pitc_get_ticks(void)
{
return pitc_ticks;
}

void at91_pitc_init(void)
{
sysc_register_isr(AT91_SYSIRQ_PIT, &pitc_isr);

*AT91C_PITC_PIMR = (AT91C_PITC_PIV & HZ_TO_PIV(100)) |
AT91C_PITC_PITEN |
AT91C_PITC_PITIEN;
}

+ 0
- 34
src/at91_rttc_test.c View File

@@ -1,34 +0,0 @@
#include <stdio.h>
#include "AT91SAM7S256.h"
#include "at91_sysc.h"
#include "board.h"

static void rtt_isr(uint32_t status)
{
*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
/*
static uint32_t i;
*AT91C_PIOA_SODR = i;
i = i ^ LED_ORANGE;
*AT91C_PIOA_CODR = i;
*/
}

void at91_rttc_test_init(void)
{
/* calculate SLOWCK from MAINCK and measured MAINF */
uint32_t prescaler = MAINCK * 16 / (*AT91C_CKGR_MCFR & AT91C_CKGR_MAINF);
sysc_register_isr(AT91_SYSIRQ_RTT, &rtt_isr);

/*
* AT91C_RTTC_RTTINCIEN doesn't work
* use AT91C_RTTC_ALMIEN and increment RTAR in isr
*/
*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
*AT91C_RTTC_RTMR = (AT91C_RTTC_RTPRES & prescaler) |
AT91C_RTTC_ALMIEN |
AT91C_RTTC_RTTRST;
printf("rttc running at %ld Hz\n\r", prescaler);
}

+ 0
- 227
src/at91_tc1.c View File

@@ -1,227 +0,0 @@
/***************************************************************************
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; version 2 of the License *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h> /* abs() */
#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_tc1.h"
#include "at91_pio.h"

/* Hard limits for ISR */
#define PULSE_MIN 0x0500
#define PULSE_MAX 0x0D00
#define PULSE_TIMEOUT 0x0F00
#define PULSE_CENTER 0x08C0

/* moving average filters */
#define PULSE_FILTER_FAST (1<<2)
#define PULSE_FILTER_SLOW (1<<4)
#define PULSE_FILTER_DIFF 16 /* point to switch filters */
#define PULSE_MID_DIFF 50 /* minimum diff to center */

#define VALUE_RANGE 256

#define ROUND_DIV256(x) ((x >> 8) + ((x & 0x80) ? 1 : 0))

struct channel_data {
uint16_t width;
uint16_t width_slow;
uint16_t filter; /* 0 - fast filter, 1 - slow filter */
uint16_t min; /* minimum value during calibration */
uint16_t mid; /* center value */
uint16_t max; /* maximum value */
};

static struct channel_data ch_data[MAX_CHANNELS];
static uint32_t count, valid, cal_in_progress;

static void ppm_isr(void)
{
static uint32_t i;

/* RC Compare -> no TIOA1 edge for 2.5ms */
uint32_t status = *AT91C_TC1_SR;

if (status & AT91C_TC_CPCS) {
/* average channel count */
count = ((count * 7) + (i << 8)) / 8;

/* at least 4 channels and a stable channel count */
if ((ROUND_DIV256(count) == i) && (i >= 4)) {
if (valid < 10)
valid++;

} else if (valid > 0) {
valid--;
}

/* reset index */
i = 0;
}

/* edge on TIOA1 */
if (status & AT91C_TC_LDRAS) {
/* get impulse width */
uint16_t width = *AT91C_TC1_RA;

/* valid range: 1 - 2ms */
if (width > PULSE_MIN && width < PULSE_MAX) {
if (i < ARRAY_SIZE(ch_data)) {
/* calc both filters */
ch_data[i].width = ((ch_data[i].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
ch_data[i].width_slow = ((ch_data[i].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;

if (cal_in_progress) {
/* use slow filter values, calc center */
ch_data[i].min = MIN(ch_data[i].width_slow, ch_data[i].min);
ch_data[i].max = MAX(ch_data[i].width_slow, ch_data[i].max);
ch_data[i].mid = (ch_data[i].min + ch_data[i].max) / 2;
}
}
i++;
}
}
}

uint32_t rcontrol_getvalues(struct rc_values *rc)
{
if (valid < 5)
return 0;

uint32_t i;
uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
for (i = 0; i < cnt; i++) {
/* switch between fast and slow filter */
uint16_t filter = (abs(ch_data[i].width - ch_data[i].width_slow) < PULSE_FILTER_DIFF);

/*
* transition fast -> slow filter
* slow filter is lagging behind, so give it a boost
*/
if (filter && !ch_data[i].filter && !cal_in_progress)
ch_data[i].width_slow = ch_data[i].width;

ch_data[i].filter = filter;

uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;

/* expand the value to +/- VALUE_RANGE */
int32_t tmp = (width - ch_data[i].mid) * VALUE_RANGE;
tmp = tmp / ((tmp > 0) ? (ch_data[i].max - ch_data[i].mid) : (ch_data[i].mid - ch_data[i].min));

/* keep result in range */
if (tmp > VALUE_RANGE)
tmp = VALUE_RANGE;

if (tmp < -VALUE_RANGE)
tmp = -VALUE_RANGE;

rc->chan[i] = tmp;
}
return cnt;
}

void rcontrol_calibrate(uint32_t mode)
{
uint32_t i;

switch (mode) {
case RC_CAL_START:
cal_in_progress = 1;
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
/* use hard limits as hint */
ch_data[i].max = PULSE_MIN;
ch_data[i].mid = (PULSE_MIN + PULSE_MAX) / 2;
ch_data[i].min = PULSE_MAX;
}
break;

case RC_CAL_END:
cal_in_progress = 0;
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
/* treat current position as center */
ch_data[i].mid = ch_data[i].width_slow;

/* if center is near minimum, clamp output to 0..+1024 */
if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
ch_data[i].mid = ch_data[i].min;

/* if center is near maximum, clamp output to -1024..0 */
if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
ch_data[i].mid = ch_data[i].max;
}
break;
}
}

void rcontrol_print_cal(void)
{
uint32_t i;
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
printf("%ld: %d(%+d) - %d(0) - %d(%+d)\n\r", i,
ch_data[i].min, ch_data[i].min - ch_data[i].mid,
ch_data[i].mid,
ch_data[i].max, ch_data[i].max - ch_data[i].mid
);
}
}

void at91_tc1_init(void)
{
/* enable TC1 clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_TC1);

/* MCK /32, trigger & capture on falling TIOA1 edge */
*AT91C_TC1_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | AT91C_TC_LDRA_FALLING |
AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG;

/* enable RA load and RC compare interrupt */
*AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;

/* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
*AT91C_TC1_RC = PULSE_TIMEOUT;

/* enable & trigger the clock */
*AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;

/* level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
aic->AIC_IECR = (1 << AT91C_ID_TC1);
}

static void tast_monitor(uint32_t status, uint32_t input)
{
if (!(input & TAST1) && (cal_in_progress == 0)) {
printf("start calibration\n\r");

rcontrol_calibrate(RC_CAL_START);


} else if (!(input & TAST2) && (cal_in_progress == 1)) {
printf("end calibration\n\r");

rcontrol_calibrate(RC_CAL_END);
rcontrol_print_cal();
}
}

PIO_PINCHANGE_ISR(TAST1 | TAST2, tast_monitor);

+ 0
- 261
src/at91_twi.c View File

@@ -1,261 +0,0 @@
/***************************************************************************
* Copyright (C) 02/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; version 2 of the License *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include <stdint.h>
#include <stdio.h>

#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_twi.h"
#include "at91_pio.h"

/*
* undocumented TWI_SR flags, at least OVRE seems to be present on a sam7s256
* taken from linux-2.6.24/include/asm-arm/arch-at91/at91_twi.h
*/
#define AT91_TWI_OVRE (1<<6) /* Overrun */
#define AT91_TWI_UNRE (1<<7) /* Underrun */

/*
* while ((cldiv = ((MCK / (2 * TWI)) -3) / (1 << ckdiv)) > 255)
* ckdiv++ ;
*
* works for TWI >= 100kHz
*/
#define TWI_CLK(x) (((MCK / (2 * (x))) -3)<<8 | ((MCK / (2 * (x))) -3))

enum twi_states {
TWI_IDLE = 0x00,
TWI_ERROR,
TWI_GENERIC_CMD = 0x10,
TWI_BLMC_UPDATE = 0x20,
};

static volatile uint32_t twi_state = TWI_IDLE;
static uint8_t *twi_data;
static uint32_t twi_size;
static uint32_t twi_count;

static void twi_isr(void)
{
/* get status */
uint32_t status = *AT91C_TWI_SR;
status &= *AT91C_TWI_IMR;

/* NACK - disable all interrupts and go to state TWI_ERROR */
if (status & AT91C_TWI_NACK) {
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
twi_state = TWI_ERROR;

// TODO: TWI_BLMC_UPDATE?
return;
}

/* tx register ready for new data */
if (status & AT91C_TWI_TXRDY) {
if (twi_count != twi_size) {
/* feed next byte */
*AT91C_TWI_THR = twi_data[twi_count++];

} else {
/* wait for TXCOMP */
*AT91C_TWI_IDR = AT91C_TWI_RXRDY | AT91C_TWI_TXRDY;
*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
}
}

/* rx register has data */
if (status & AT91C_TWI_RXRDY) {
/* get data */
twi_data[twi_count++] = *AT91C_TWI_RHR;

/* transfer complete? */
if (twi_count == twi_size) {
/* send STOP and wait for TXCOMP */
*AT91C_TWI_CR = AT91C_TWI_STOP;
*AT91C_TWI_IDR = AT91C_TWI_TXRDY;
*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
}
}

/* transfer really complete? */
if (status & AT91C_TWI_TXCOMP) {

/* are we doing a blmc update? */
if (twi_state == TWI_BLMC_UPDATE) {
uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F;
if (addr != TWI_ADDR_BL4) {
/* increase address */
*AT91C_TWI_MMR += (1<<16);

/* send next value to next blmc */
*AT91C_TWI_THR = *twi_data++;
} else {
// TODO:
}

} else {
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
twi_state = TWI_IDLE;
}
}
}

uint32_t twi_setpwm(uint8_t *values)
{
if (twi_state != TWI_IDLE)
return 1;

twi_state = TWI_BLMC_UPDATE;
twi_data = values; /* data is not copied! */
twi_size = 0;
twi_count = 0;

*AT91C_TWI_MMR = (TWI_ADDR_BL1 << 16) | AT91C_TWI_IADRSZ_1_BYTE;
*AT91C_TWI_IADR = CMD_SET_PWM;

*AT91C_TWI_THR = *twi_data++;
*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;

return 0;
}

uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
{
if (twi_state != TWI_IDLE)
return 1;

/* TODO: locking needed? */
twi_state = TWI_GENERIC_CMD;

/* read transfer, or write transfer with payload */
if (cmd->mode & BLMC_CMD_READ || cmd->size != 0) {
/* set address, direction, argument count and command bytes */
*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
*AT91C_TWI_IADR = cmd->cmd;

/* write transfer without payload */
} else {
/* use one cmd byte as payload (needed to start transfer) */
cmd->mode--;
*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
*AT91C_TWI_IADR = (cmd->cmd) >> 8;
}

/* isr needs data & size parameters */
twi_data = cmd->data;
twi_size = cmd->size;
twi_count = 0;

if (cmd->mode & BLMC_CMD_READ) {
*AT91C_TWI_CR = AT91C_TWI_START;
*AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK;

} else {
*AT91C_TWI_THR = (twi_size != 0) ? cmd->data[twi_count++] : (cmd->cmd & 0xFF);
*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
}

/*
* wait for end
* TODO: locking needed?
* TODO: timeout?
*/
while (twi_state != TWI_IDLE && twi_state != TWI_ERROR);
if (twi_state != TWI_IDLE) {
twi_state = TWI_IDLE;
return 1;
}

return 0;
}

void at91_twi_test(void)
{
uint32_t i;
for (i = TWI_ADDR_BL1; i <= TWI_ADDR_BL4; i++) {
struct blmc_cmd cmd = {
.cmd = CMD_BOOT_LOADER,
.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG,
};
uint32_t ret = twi_cmd(i, &cmd);
printf("twi[0x%02lx](%ld) ", i, ret);

volatile uint32_t x;
for (x = 0; x < 200000; x++);

uint8_t buf[16];
cmd.cmd = CMD_GET_INFO,
cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG,
cmd.size = sizeof(buf),
cmd.data = buf,
ret = twi_cmd(i, &cmd);
printf("boot(%ld):'%s' ", ret, buf);

cmd.cmd = CMD_GET_SIGNATURE;
cmd.size = 4;
ret = twi_cmd(i, &cmd);
printf("sig(%ld):0x%02x%02x%02x ", ret, buf[0], buf[1], buf[2]);

cmd.cmd = CMD_BOOT_APPLICATION;
cmd.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG;
cmd.size = 0;
ret = twi_cmd(i, &cmd);

for (x = 0; x < 200000; x++);

cmd.cmd = CMD_GET_INFO,
cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG,
cmd.size = sizeof(buf),
cmd.data = buf,
ret = twi_cmd(i, &cmd);
printf("app(%ld):'%s'\n\r", ret, buf);
}
}

void at91_twi_init(void)
{
/* enable Clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_TWI);

/* SDA & SCL from Peripheral A, Open Drain, no Pullup */
AT91S_PIO *pio = AT91C_BASE_PIOA;

/* do a software reset (bus not connected) */
*AT91C_TWI_CR = AT91C_TWI_SWRST;

pio->PIO_MDER = AT91C_PA3_TWD | AT91C_PA4_TWCK;
pio->PIO_PPUDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
pio->PIO_ASR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
pio->PIO_PDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;

/* set TWI Clock */
*AT91C_TWI_CWGR = TWI_CLK(400000); //| (5<<16);

/* disable all (known) interrupts */
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;

/* level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_TWI] = IRQPRIO_TWI | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_TWI] = (uint32_t)twi_isr;
aic->AIC_IECR = (1 << AT91C_ID_TWI);

/* enable teh monster */
*AT91C_TWI_CR = AT91C_TWI_MSEN;
}

+ 0
- 552
src/at91_udp.c View File

@@ -1,552 +0,0 @@
/***************************************************************************
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; version 2 of the License *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include <stdio.h>
#include "AT91SAM7S256.h"
#include "at91_pio.h"
#include "board.h"
#include "fifo.h"

#include "usb_ch9.h"
#include "usb_cdc.h"

#define csr_clear_flags(csr, flags) \
while ((csr) & (flags)) \
(csr) &= ~(flags);

#define csr_set_flags(csr, flags) \
while (((csr) & (flags)) != (flags)) \
(csr) |= (flags);

struct ep_transfer {
uint16_t length;
uint16_t curpos;

char *data;
void (*complete_cb)(void);
};

struct ep_ctx {
uint16_t maxpktsize;
uint16_t flags;

union {
struct ep_transfer *transfer;
struct fifo *fifo;
};
};

#define CTX_TRANSFER 0x01 /* ctx use ep_transfer struct */
#define CTX_FIFO 0x02 /* ctx use fifo */
#define CTX_IN 0x04 /* write to the host */
#define CTX_OUT 0x08 /* read from the host */
#define CTX_RXBANK0 0x10
#define CTX_RXBANK1 0x20

static struct ep_transfer ep0_transfer;
static struct ep_ctx ep_ctx[4];

static uint16_t current_address;
static uint16_t current_config;
static uint16_t current_interface;

static const struct usb_device_descriptor dev_descriptor = {
.bLength = sizeof(struct usb_device_descriptor),
.bDescriptorType = USB_DT_DEVICE,
.bcdUSB = 0x0110,
.bMaxPacketSize0 = 8,
.idVendor = USB_VENDOR_ID,
.idProduct = USB_PRODUCT_ID +1,
.bcdDevice = 0x0001,
.bNumConfigurations = 1,
};

struct my_config {
struct usb_config_descriptor cfg;
struct usb_interface_descriptor ctrl_iface;
struct usb_cdc_header_desc cdc_header;
struct usb_cdc_call_mgmt_descriptor cdc_call_mgmt;
struct usb_cdc_acm_descriptor cdc_acm;
struct usb_cdc_union_desc cdc_union;
struct usb_endpoint_descriptor notify_ep;
struct usb_interface_descriptor data_iface;
struct usb_endpoint_descriptor dataout_ep;
struct usb_endpoint_descriptor datain_ep;
} __attribute__ ((packed));

static const struct my_config cfg_descriptor = {
.cfg = {
.bLength = sizeof(struct usb_config_descriptor),
.bDescriptorType = USB_DT_CONFIG,
.wTotalLength = sizeof(struct my_config),
.bNumInterfaces = 2,
.bConfigurationValue = 1,
.bmAttributes = USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP,
.bMaxPower = 50,
},
.ctrl_iface = {
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bNumEndpoints = 1,
.bInterfaceClass = USB_CLASS_COMM,
.bInterfaceSubClass = USB_CDC_SUBCLASS_ACM,
.bInterfaceProtocol = 1,
},
.cdc_header = {
.bLength = sizeof(struct usb_cdc_header_desc),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_HEADER_TYPE,
.bcdCDC = 0x0110,
},
.cdc_call_mgmt = {
.bLength = sizeof(struct usb_cdc_call_mgmt_descriptor),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_CALL_MANAGEMENT_TYPE,
.bmCapabilities = USB_CDC_CALL_MGMT_CAP_CALL_MGMT,
.bDataInterface = 1,
},
.cdc_acm = {
.bLength = sizeof(struct usb_cdc_acm_descriptor),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_ACM_TYPE,
.bmCapabilities = (USB_CDC_CAP_BRK | USB_CDC_CAP_LINE | USB_CDC_COMM_FEATURE),
},
.cdc_union = {
.bLength = sizeof(struct usb_cdc_union_desc),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_UNION_TYPE,
.bMasterInterface0 = 0,
.bSlaveInterface0 = 1,
},
.notify_ep = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN | 0x03,
.bmAttributes = USB_ENDPOINT_XFER_INT,
.wMaxPacketSize = 64,
.bInterval = 10,
},
.data_iface = {
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 1,
.bNumEndpoints = 2,
.bInterfaceClass = USB_CLASS_CDC_DATA,
},
.dataout_ep = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_OUT | 0x01,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = 64,
},
.datain_ep = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN | 0x02,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = 64,
},
};

static void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
void (*complete_cb)(void))
{
struct ep_ctx *ctx = &ep_ctx[ep];
// printf("ep_transfer_send(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
return;

/* from buffer to usb */
ctx->flags |= CTX_IN;

struct ep_transfer *transfer = ctx->transfer;
transfer->length = length;
transfer->curpos = 0;
transfer->data = data;
transfer->complete_cb = complete_cb;

uint32_t maxsize = ctx->maxpktsize;

/* get data from transfer */
while (transfer->curpos < transfer->length && maxsize--)
AT91C_UDP_FDR[ep] = transfer->data[transfer->curpos++];

/* trigger tx */
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
}

static void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
void (*complete_cb)(void))
{
struct ep_ctx *ctx = &ep_ctx[ep];
// printf("ep_transfer_receive(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
return;

/* from usb to buffer */
ctx->flags |= CTX_OUT;

struct ep_transfer *transfer = ctx->transfer;
transfer->length = length;
transfer->curpos = 0;
transfer->data = data;
transfer->complete_cb = complete_cb;
}

/* stalls the endpoint */
static void ep_send_stall(uint32_t ep)
{
printf("stall\n\r");
AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL;
}

static void udp_configure_ep(const struct usb_endpoint_descriptor *desc)
{
/* get endpoint address, set Max Packet Size */
uint32_t ep = desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
ep_ctx[ep].maxpktsize = desc->wMaxPacketSize;

/* get endpoint type (ctrl, iso, bulb, int) */
uint32_t eptype = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
if (desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK) {
eptype |= 0x04;
} else {
ep_ctx[ep].flags |= CTX_RXBANK0;
}

/* configure UDP endpoint and enable interrupt */
AT91C_UDP_CSR[ep] = AT91C_UDP_EPEDS | (eptype << 8);
*AT91C_UDP_IER = (1 << ep);
}

/*
* set local address
* (USB_REQ_SET_ADDRESS callback)
*/
static void udp_txcb_setaddress(void)
{
*AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address);
*AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN;
}

/*
* configure endpoints
* (USB_REQ_SET_CONFIGURATION callback)
*/
static void udp_txcb_setconfig(void)
{
udp_configure_ep(&cfg_descriptor.notify_ep);
udp_configure_ep(&cfg_descriptor.datain_ep);
udp_configure_ep(&cfg_descriptor.dataout_ep);

/* set UDP to "configured" */
*AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG;
}

static void udp_txcb_setinterface(void)
{
printf("claim interface %d\n\r", current_interface);
}

static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
{
printf("typ:0x%02x req:0x%02x val:0x%04x idx:0x%04x len:0x%04x\n\r",
req->bRequestType, req->bRequest, req->wValue, req->wIndex, req->wLength);

switch (req->bRequestType & (USB_TYPE_MASK | USB_RECIP_MASK)) {
case (USB_TYPE_STANDARD | USB_RECIP_DEVICE): /* 0x00/0x80 */
switch (req->bRequest) {
case USB_REQ_SET_ADDRESS: /* 0x05 */
current_address = req->wValue;
ep_transfer_send(0, NULL, 0, udp_txcb_setaddress);
break;

case USB_REQ_GET_DESCRIPTOR: /* 0x06 */
switch (req->wValue >> 8) {
case USB_DT_DEVICE: /* 0x01 */
ep_transfer_send(0, (char *)&dev_descriptor,
MIN(sizeof(dev_descriptor), req->wLength),
NULL);
break;

case USB_DT_CONFIG: /* 0x02 */
ep_transfer_send(0, (char *)&cfg_descriptor,
MIN(sizeof(cfg_descriptor), req->wLength),
NULL);
break;

default:
ep_send_stall(0);
break;
}
break;

case USB_REQ_SET_CONFIGURATION: /* 0x09 */
current_config = req->wValue;
ep_transfer_send(0, NULL, 0, udp_txcb_setconfig);
break;

default:
ep_send_stall(0);
break;
}
break;

case (USB_TYPE_STANDARD | USB_RECIP_INTERFACE): /* 0x01/0x81 */
// TODO: follow current_interface
switch (req->bRequest) {
case USB_REQ_SET_INTERFACE: /* 0x0b */
current_interface = req->wValue;
ep_transfer_send(0, NULL, 0, udp_txcb_setinterface);
break;

default:
ep_send_stall(0);
break;
}
break;

case (USB_TYPE_CLASS | USB_RECIP_INTERFACE): /* 0x21/0xA1 */
// TODO: follow current_interface
switch (req->bRequest) {
case USB_CDC_REQ_SET_LINE_CODING: /* 0x20 */
/* read 7 bytes */
break;

case USB_CDC_REQ_SET_CONTROL_LINE_STATE: /* 0x22 */
ep_transfer_send(0, NULL, 0, NULL);
break;

default:
ep_send_stall(0);
break;
}
break;

default:
ep_send_stall(0);
break;
}
}

static void udp_handle_ep(uint32_t ep)
{
/* endpoint enabled? */
AT91_REG *csr = &AT91C_UDP_CSR[ep];
if (!(*csr & AT91C_UDP_EPEDS))
return;

/* ctrl request packet? */
if (*csr & AT91C_UDP_RXSETUP) {
struct usb_ctrlrequest req;
uint8_t *p;
for (p = (uint8_t *)&req; p < (uint8_t *)(&req +1); p++)
*p = AT91C_UDP_FDR[ep];

/* set data phase transfer direction */
if (req.bRequestType & USB_DIR_IN)
*csr |= AT91C_UDP_DIR;

/* clear interrupt - *MUST* use csr_clear_flags() here */
csr_clear_flags(*csr, AT91C_UDP_RXSETUP);

ep_handle_ctrlrequest(&req);
}

/* transmit complete? */
if (*csr & AT91C_UDP_TXCOMP) {
struct ep_ctx *ctx = &ep_ctx[ep];

if (ctx->flags & CTX_FIFO) {
/* get data from fifo */
if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize))
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;

} else if ((ctx->flags & (CTX_TRANSFER | CTX_IN)) == (CTX_TRANSFER | CTX_IN)) {
/* transfer not complete */
struct ep_transfer *transfer = ctx->transfer;
if (transfer->length != transfer->curpos) {
uint32_t maxsize = ctx->maxpktsize;

/* get data from transfer */
while (transfer->curpos < transfer->length && maxsize--)
AT91C_UDP_FDR[ep] = transfer->data[transfer->curpos++];

/* trigger tx */
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;

/* transfer complete, execute callback */
} else {
ctx->flags &= ~CTX_IN;

if (transfer->complete_cb)
transfer->complete_cb();
}
}

/* clear interrupt */
*csr &= ~(AT91C_UDP_TXCOMP);
}

/* clear STALLSENT interrupt */
if (*csr & AT91C_UDP_STALLSENT)
csr_clear_flags(*csr, (AT91C_UDP_STALLSENT | AT91C_UDP_FORCESTALL));

/* data ready to read? */
if (*csr & (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1)) {
struct ep_ctx *ctx = &ep_ctx[ep];

uint16_t len = (*csr & AT91C_UDP_RXBYTECNT) >> 16;

// TODO: only ep0 status OUT?
if (!len && (ctx->flags & CTX_TRANSFER)) {
ctx->flags &= ~(CTX_OUT | CTX_IN);
ctx->transfer->length = 0;
ctx->transfer->curpos = 0;
}

if (ctx->flags & CTX_FIFO) {
fifo_rxudp(ctx->fifo, ep, len);

} else if ((ctx->flags & (CTX_TRANSFER | CTX_OUT)) == (CTX_TRANSFER | CTX_OUT)) {

/* transfer not complete */
struct ep_transfer *transfer = ctx->transfer;
if (transfer->length != transfer->curpos) {
/* get data from transfer */
while (transfer->curpos < transfer->length && len--)
transfer->data[transfer->curpos++] = AT91C_UDP_FDR[ep];
}

/* test again */
if (transfer->length == transfer->curpos) {
ctx->flags &= ~CTX_OUT;

if (transfer->complete_cb)
transfer->complete_cb();
}
}

if (ctx->flags & CTX_RXBANK0) {
if (*csr & AT91C_UDP_RX_DATA_BK0)
csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK0);

/* all but ep0 have ping pong buffers */
if (ep > 0)
ctx->flags = (ctx->flags & ~CTX_RXBANK0) | CTX_RXBANK1;

} else if (ctx->flags & CTX_RXBANK1) {
if (*csr & AT91C_UDP_RX_DATA_BK1)
csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK1);

ctx->flags = (ctx->flags & ~CTX_RXBANK1) | CTX_RXBANK0;
}
}
}

static void udp_isr(void)
{
uint32_t isr = *AT91C_UDP_ISR;
if (isr & AT91C_UDP_ENDBUSRES) {
AT91S_UDP *udp = AT91C_BASE_UDP;

/* reset all endpoints */
udp->UDP_RSTEP = (AT91C_UDP_EP0 | AT91C_UDP_EP1 |
AT91C_UDP_EP2 | AT91C_UDP_EP3) ;
udp->UDP_RSTEP = 0;

/* init ep0 */
struct ep_ctx *ctx = &ep_ctx[0];
ctx->maxpktsize = 8;
ctx->flags = CTX_TRANSFER | CTX_RXBANK0;
ctx->transfer = &ep0_transfer;
ctx->transfer->length = 0;
ctx->transfer->curpos = 0;

/* Configure endpoint0 as Control EP */
udp->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);

/* enable ep0 Interrupt, disable all others */
udp->UDP_IER = AT91C_UDP_EPINT0;
udp->UDP_IDR = AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 | AT91C_UDP_EPINT3 |
AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
AT91C_UDP_WAKEUP;
}

/* Handle Endpoint Interrupts */
uint32_t i;
for (i = 0; i < 4; i++) {
if (isr & *AT91C_UDP_IMR & (1<<i))
udp_handle_ep(i);
}

/* clear all unhandled interrupts */
*AT91C_UDP_ICR = isr & (AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM |
AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP);
}

void at91_udp_init(void)
{
/* configure & disable Pullup, disable Pullup von VBUS */
AT91PS_PIO pio = AT91C_BASE_PIOA;
pio->PIO_CODR = UDP_PULLUP;
pio->PIO_PER = UDP_PULLUP;
pio->PIO_OER = UDP_PULLUP;
// TODO: needed?
pio->PIO_PPUDR = UDP_VBUS_MON;

/* UDPCK (48MHz) = PLLCK / 2 */
*AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1;

/* enable UDP clocks */
*AT91C_PMC_SCER = AT91C_PMC_UDP;
*AT91C_PMC_PCER = (1 << AT91C_ID_UDP);

/* enable transmitter */
*AT91C_UDP_TXVC &= ~AT91C_UDP_TXVDIS;

/* clear & disable all UDP interrupts */
*AT91C_UDP_IDR = AT91C_UDP_EPINT0 | AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 |
AT91C_UDP_EPINT3 | AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM |
AT91C_UDP_SOFINT | AT91C_UDP_WAKEUP;

*AT91C_UDP_ICR = AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP ;

/* level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_UDP] = IRQPRIO_UDP | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_UDP] = (uint32_t)udp_isr;
aic->AIC_IECR = (1 << AT91C_ID_UDP);

pio_trigger_isr(UDP_VBUS_MON);
}

static void udp_vbus_monitor(uint32_t status, uint32_t input)
{
if (input & UDP_VBUS_MON)
/* usb connected -> enable pullup */
*AT91C_PIOA_SODR = UDP_PULLUP;
else
/* usb got diconnected -> disable pullup */
*AT91C_PIOA_CODR = UDP_PULLUP;
}

PIO_PINCHANGE_ISR(UDP_VBUS_MON, udp_vbus_monitor);

+ 308
- 0
src/rtos/context.c View File

@@ -0,0 +1,308 @@
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "atomic.h"
#include "memalloc.h"

#include "rtos/context.h"
#include "rtos/spinlock.h"

/* linked list of ready contexts, #1 is the running thread */
struct context *volatile run_queue = NULL;

/* pointer to the running thread */
struct context *current_context = NULL;

/*
* SWI_Handler:
* Entry with SVC mode, IRQs are disabled, FIQ is enabled
*/
__attribute__((naked)) void SWI_Handler(void)
{
/* store register context to current_context */
asm volatile (
/* r0-1 -> svc_stack */
"stmdb sp!, {r0-r1} \n\t"

/* get top of struct register_context */
"ldr r0, =current_context \n\t"
"ldr r0, [r0] \n\t"
"add r0, r0, #68 \n\t"

/* save usermode cpsr & r2-14 */
"mrs r1, spsr \n\t"
"stmdb r0, {r1-r14}^ \n\t"
"nop \n\t"
"sub r0, r0, #56 \n\t"

/* save r0-1 and svc_lr (= pc) */
"ldmia sp!, {r1, r2} \n\t"
"stmdb r0!, {r1, r2, r14} \n\t"
);

/* we're no longer #1 in run_queue, switch to new #1 */
if (current_context != run_queue)
current_context = run_queue;

current_context->state = CONTEXT_RUNNING;

if (((uint32_t *)current_context->stack)[0] != 0xdeadbeef)
printf("<- task stack corrupt (%p)\n\r", current_context);

/* restore register context from current_context */
asm volatile (
/* get pointer to struct register_context */
"ldr r0, =current_context \n\t"
"ldr r0, [r0] \n\t"

/* get values of r0-1 and pc (= svc_lr) */
"ldmia r0!, {r1-r2,r14} \n\t"
"stmdb sp!, {r1-r2} \n\t"

/* restore usermode cpsr & r2-14 */
"ldmia r0, {r1-r14}^ \n\t"
"nop \n\t"
"msr spsr, r1 \n\t"

/* get r0-1 from svc_stack, jump back */
"ldmia sp!, {r0, r1} \n\t"
"movs pc, lr \n\t"
);
}

static uint8_t isr_context_switch(void)
{
asm volatile ("swi");

// TODO: return previous state of now running thread
return CONTEXT_READY;
}

/* inserts context into run_queue */
static void isr_context_ready(struct context *ctx)
{
struct context *q = run_queue;
struct context *volatile *qprev = &run_queue;

while (q && (q->priority <= ctx->priority)) {
qprev = &q->run_queue;
q = q->run_queue;
}

ctx->run_queue = q;
*qprev = ctx;
}

/* process yields, try to switch to process with lower or same prio */
void isr_context_yield(void)
{
run_queue = current_context->run_queue;
isr_context_ready(current_context);

if (current_context != run_queue) {
current_context->state = CONTEXT_READY;
isr_context_switch();
}
}

void context_yield(void)
{
disable_irqs();
isr_context_yield();
restore_irqs();
}

static uint8_t __isr_context_wait(struct spinlock *lock, uint8_t sleepstate)
{
isr_spinlock_unlock(lock);

run_queue = current_context->run_queue;
current_context->state = sleepstate;

uint8_t retval = isr_context_switch();

isr_spinlock_lock(lock);

return retval;
}

uint8_t isr_context_wait(struct spinlock *lock)
{
return __isr_context_wait(lock, CONTEXT_SLEEP);
}

uint8_t context_wait(struct spinlock *lock)
{
disable_irqs();
uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP);
restore_irqs();

return retval;
}

uint8_t context_wait_queue(struct spinlock *lock, struct context **queue)
{
disable_irqs();
current_context->sleep_queue = *queue;
*queue = current_context;

uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE);
restore_irqs();

return retval;
}

uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue)
{
disable_irqs();

struct context *q = *queue;
while (q && (q->priority <= lock->priority_unlocked)) {
queue = &q->sleep_queue;
q = q->sleep_queue;
}

current_context->sleep_queue = q;
*queue = current_context;

uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE);
restore_irqs();

return retval;
}

void isr_context_signal(struct context *c)
{
if (c->state == CONTEXT_SLEEP) {
c->state = CONTEXT_READY;
isr_context_ready(c);
}
}

void context_signal(struct context *c)
{
disable_irqs();