Index: kernel/arch/arm32/Makefile.inc =================================================================== --- kernel/arch/arm32/Makefile.inc (revision 04cb6957dc283940ca49fb7fa71d84ccfcd446bd) +++ kernel/arch/arm32/Makefile.inc (revision 435c33b89a901ae91e11be811e030146ad3c5cc7) @@ -37,5 +37,6 @@ ifeq ($(CONFIG_FPU),y) # This is necessary to allow vmsr insn and fpexc manipulation -GCC_CFLAGS += -mfloat-abi=hard +# Use vfp32 to allow context save/restore of d16-d31 regs. +GCC_CFLAGS += -mfloat-abi=hard -mfpu=vfp3 endif @@ -51,4 +52,5 @@ arch/$(KARCH)/src/machine_func.c \ arch/$(KARCH)/src/context.S \ + arch/$(KARCH)/src/fpu_context.c \ arch/$(KARCH)/src/dummy.S \ arch/$(KARCH)/src/cpu/cpu.c \ Index: kernel/arch/arm32/include/cpu.h =================================================================== --- kernel/arch/arm32/include/cpu.h (revision 04cb6957dc283940ca49fb7fa71d84ccfcd446bd) +++ kernel/arch/arm32/include/cpu.h (revision 435c33b89a901ae91e11be811e030146ad3c5cc7) @@ -43,5 +43,5 @@ /** Struct representing ARM CPU identification. */ typedef struct { - /** Implementator (vendor) number. */ + /** Implementor (vendor) number. */ uint32_t imp_num; Index: kernel/arch/arm32/src/cpu/cpu.c =================================================================== --- kernel/arch/arm32/src/cpu/cpu.c (revision 04cb6957dc283940ca49fb7fa71d84ccfcd446bd) +++ kernel/arch/arm32/src/cpu/cpu.c (revision 435c33b89a901ae91e11be811e030146ad3c5cc7) @@ -38,43 +38,38 @@ #include #include -#include -/** Number of indexes left out in the #imp_data array */ -#define IMP_DATA_START_OFFSET 0x40 - -/** Implementors (vendor) names */ -static const char *imp_data[] = { - "?", /* IMP_DATA_START_OFFSET */ - "ARM Limited", /* 0x41 */ - "", "", /* 0x42 - 0x43 */ - "Digital Equipment Corporation", /* 0x44 */ - "", "", "", "", "", "", "", "", /* 0x45 - 0x4c */ - "Motorola, Freescale Semicondutor Inc.", /* 0x4d */ - "", "", "", /* 0x4e - 0x50 */ - "Qualcomm Inc.", /* 0x51 */ - "", "", "", "", /* 0x52 - 0x55 */ - "Marvell Semiconductor", /* 0x56 */ - "", "", "", "", "", "", "", "", "", "", /* 0x57 - 0x60 */ - "", "", "", "", "", "", "", "", /* 0x61 - 0x68 */ - "Intel Corporation" /* 0x69 */ -}; - -/** Length of the #imp_data array */ -static const unsigned int imp_data_length = sizeof(imp_data) / sizeof(char *); +/** Implementers (vendor) names */ +static const char * implementer(unsigned id) +{ + switch (id) + { + case 0x41: return "ARM Limited"; + case 0x44: return "Digital Equipment Corporation"; + case 0x4d: return "Motorola, Freescale Semiconductor Inc."; + case 0x51: return "Qualcomm Inc."; + case 0x56: return "Marvell Semiconductor Inc."; + case 0x69: return "Intel Corporation"; + } + return "Unknown implementer"; +} /** Architecture names */ -static const char *arch_data[] = { - "?", /* 0x0 */ - "4", /* 0x1 */ - "4T", /* 0x2 */ - "5", /* 0x3 */ - "5T", /* 0x4 */ - "5TE", /* 0x5 */ - "5TEJ", /* 0x6 */ - "6" /* 0x7 */ -}; - -/** Length of the #arch_data array */ -static const unsigned int arch_data_length = sizeof(arch_data) / sizeof(char *); +static const char * architecture_string(cpu_arch_t *arch) +{ + static const char *arch_data[] = { + "ARM", /* 0x0 */ + "ARMv4", /* 0x1 */ + "ARMv4T", /* 0x2 */ + "ARMv5", /* 0x3 */ + "ARMv5T", /* 0x4 */ + "ARMv5TE", /* 0x5 */ + "ARMv5TEJ", /* 0x6 */ + "ARMv6" /* 0x7 */ + }; + if (arch->arch_num < (sizeof(arch_data) / sizeof(arch_data[0]))) + return arch_data[arch->arch_num]; + else + return arch_data[0]; +} @@ -82,4 +77,5 @@ * * @param cpu Structure for storing CPU identification. + * See page B4-1630 of ARM Architecture Reference Manual. */ static void arch_cpu_identify(cpu_arch_t *cpu) @@ -96,4 +92,5 @@ cpu->prim_part_num = (ident << 16) >> 20; cpu->rev_num = (ident << 28) >> 28; + // TODO CPUs with arch_num == 0xf use CPUID scheme for identification } @@ -139,54 +136,4 @@ } -void fpu_init(void) -{ - //TODO: Identify FPU unit - //and set correct functions to save/restore ctx -} - -void fpu_enable(void) -{ - /* Enable FPU instructions */ - asm volatile ( - "ldr r1, =0x40000000\n" - "vmsr fpexc, r1\n" - ::: "r1" - ); -} - -void fpu_disable(void) -{ - /* Disable FPU instructions */ - asm volatile ( - "ldr r1, =0x00000000\n" - "vmsr fpexc, r1\n" - ::: "r1" - ); -} - -void fpu_context_save(fpu_context_t *ctx) -{ - // TODO check and complete. What about fpexc? - asm volatile ( - "vmrs r1, fpscr\n" -// "vmrs r2, fpexc\n" - "stm %0, {r1, r2}\n" - "vstm %0, {d0-d15}\n" - ::"r" (ctx): "r1","r2","memory" - ); -} - -void fpu_context_restore(fpu_context_t *ctx) -{ - // TODO check and complete. What about fpexc? - asm volatile ( - "ldm %0, {r1, r2}\n" - "vmsr fpscr, r1\n" -// "vmsr fpexc, r2\n" - "vldm %0, {d0-d15}\n" - ::"r" (ctx): "r1","r2" - ); -} - /** Retrieves processor identification and stores it to #CPU.arch */ void cpu_identify(void) @@ -198,23 +145,9 @@ void cpu_print_report(cpu_t *m) { - const char *vendor = imp_data[0]; - const char *architecture = arch_data[0]; - cpu_arch_t * cpu_arch = &m->arch; - - const unsigned imp_offset = cpu_arch->imp_num - IMP_DATA_START_OFFSET; - - if (imp_offset < imp_data_length) { - vendor = imp_data[cpu_arch->imp_num - IMP_DATA_START_OFFSET]; - } - - // TODO CPUs with arch_num == 0xf use CPUID scheme for identification - if (cpu_arch->arch_num < arch_data_length) { - architecture = arch_data[cpu_arch->arch_num]; - } - - printf("cpu%d: vendor=%s, architecture=ARMv%s, part number=%x, " + printf("cpu%d: vendor=%s, architecture=%s, part number=%x, " "variant=%x, revision=%x\n", - m->id, vendor, architecture, cpu_arch->prim_part_num, - cpu_arch->variant_num, cpu_arch->rev_num); + m->id, implementer(m->arch.imp_num), + architecture_string(&m->arch), m->arch.prim_part_num, + m->arch.variant_num, m->arch.rev_num); } Index: kernel/arch/arm32/src/fpu_context.c =================================================================== --- kernel/arch/arm32/src/fpu_context.c (revision 435c33b89a901ae91e11be811e030146ad3c5cc7) +++ kernel/arch/arm32/src/fpu_context.c (revision 435c33b89a901ae91e11be811e030146ad3c5cc7) @@ -0,0 +1,208 @@ +/* + * Copyright (c) 2012 Jan Vesely + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * - Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * - The name of the author may not be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** @addtogroup arm32 + * @{ + */ +/** @file + * @brief arm32 FPU context + */ + +#include +#include +#include + +#define FPSID_IMPLEMENTER(r) ((r) >> 24) +#define FPSID_SW_ONLY_FLAG (1 << 23) +#define FPSID_SUBACHITECTURE(r) (((r) >> 16) & 0x7f) +#define FPSID_PART_NUMBER(r) (((r) >> 8) & 0xff) +#define FPSID_VARIANT(r) (((r) >> 4) 0xf) +#define FPSID_REVISION(r) (((r) >> 0) 0xf) +enum { + FPU_VFPv1 = 0x00, + FPU_VFPv2_COMMONv1 = 0x01, + FPU_VFPv3_COMMONv2 = 0x02, /* Check MVFR0 and MVFR 1*/ + FPU_VFPv3_NOTRAP = 0x3, /* Does not support trap */ + FPU_VFPv3 = 0x4, +}; + +static void (*save_context)(fpu_context_t *ctx); +static void (*restore_context)(fpu_context_t *ctx); + +/** Saves 32 single precision fpu registers. + * @param ctx FPU context area. + * Used by VFPv1 + */ +static void fpu_context_save_s32(fpu_context_t *ctx) +{ + asm volatile ( + "vmrs r1, fpscr\n" + "stm %0, {r1}\n" + "vstm %0, {s0-s31}\n" + ::"r" (ctx): "r1","memory" + ); +} + +/** Restores 32 single precision fpu registers. + * @param ctx FPU context area. + * Used by VFPv1 + */ +static void fpu_context_restore_s32(fpu_context_t *ctx) +{ + asm volatile ( + "ldm %0, {r1}\n" + "vmsr fpscr, r1\n" + "vldm %0, {s0-s31}\n" + ::"r" (ctx): "r1" + ); +} + +/** Saves 16 double precision fpu registers. + * @param ctx FPU context area. + * Used by VFPv2, VFPv3-d16, and VFPv4-d16. + */ +static void fpu_context_save_d16(fpu_context_t *ctx) +{ + asm volatile ( + "vmrs r1, fpscr\n" + "stm %0, {r1}\n" + "vstm %0, {d0-d15}\n" + ::"r" (ctx): "r1","memory" + ); +} + +/** Restores 16 double precision fpu registers. + * @param ctx FPU context area. + * Used by VFPv2, VFPv3-d16, and VFPv4-d16. + */ +static void fpu_context_restore_d16(fpu_context_t *ctx) +{ + asm volatile ( + "ldm %0, {r1}\n" + "vmsr fpscr, r1\n" + "vldm %0, {d0-d15}\n" + ::"r" (ctx): "r1" + ); +} + +/** Saves 32 double precision fpu registers. + * @param ctx FPU context area. + * Used by VFPv3-d32, VFPv4-d32, and advanced SIMD. + */ +static void fpu_context_save_d32(fpu_context_t *ctx) +{ + asm volatile ( + "vmrs r1, fpscr\n" + "stm %0, {r1}\n" + "vstm %0, {d0-d15}\n" + "vstm %0, {d16-d31}\n" + ::"r" (ctx): "r1","memory" + ); +} + +/** Restores 32 double precision fpu registers. + * @param ctx FPU context area. + * Used by VFPv3-d32, VFPv4-d32, and advanced SIMD. + */ +static void fpu_context_restore_d32(fpu_context_t *ctx) +{ + asm volatile ( + "ldm %0, {r1}\n" + "vmsr fpscr, r1\n" + "vldm %0, {d0-d15}\n" + "vldm %0, {d16-d31}\n" + ::"r" (ctx): "r1" + ); +} + +void fpu_init(void) +{ + uint32_t fpsid = 0; + uint32_t mvfr0 = 0; + asm volatile ( + "vmrs %0,fpsid\n" + "vmrs %1,mvfr0\n" + :"=r"(fpsid), "=r"(mvfr0):: + ); + //TODO: Identify FPU unit + //and set correct functions to save/restore ctx + switch (FPSID_SUBACHITECTURE(fpsid)) + { + case FPU_VFPv1: + save_context = fpu_context_save_s32; + restore_context = fpu_context_restore_s32; + break; + case FPU_VFPv2_COMMONv1: + save_context = fpu_context_save_d16; + restore_context = fpu_context_restore_d16; + break; + case FPU_VFPv3_COMMONv2: + case FPU_VFPv3_NOTRAP: + case FPU_VFPv3: + /* See page B4-1637 */ + if ((mvfr0 & 0xf) == 0x1) { + save_context = fpu_context_save_d32; + restore_context = fpu_context_restore_d32; + } else { + save_context = fpu_context_save_d16; + restore_context = fpu_context_restore_d16; + } + break; + + } +} + +void fpu_enable(void) +{ + /* Enable FPU instructions */ + asm volatile ( + "ldr r1, =0x40000000\n" + "vmsr fpexc, r1\n" + ::: "r1" + ); +} + +void fpu_disable(void) +{ + /* Disable FPU instructions */ + asm volatile ( + "ldr r1, =0x00000000\n" + "vmsr fpexc, r1\n" + ::: "r1" + ); +} + +void fpu_context_save(fpu_context_t *ctx) +{ + save_context(ctx); +} + +void fpu_context_restore(fpu_context_t *ctx) +{ + restore_context(ctx); +}