File indexing completed on 2025-05-11 08:23:44
0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012 #define RTEMS_STATUS_CHECKS_USE_PRINTK
0013
0014 #include <stdlib.h>
0015 #include <stdio.h>
0016 #include <errno.h>
0017 #include <sys/types.h>
0018 #include <rtems.h>
0019 #include <bsp.h>
0020 #include <bsp/irq-generic.h>
0021 #include <rtems/libio.h>
0022 #include <rtems/status-checks.h>
0023 #include "../include/system_conf.h"
0024 #include <bsp/milkymist_pfpu.h>
0025
0026 #define DEVICE_NAME "/dev/pfpu"
0027
0028 static rtems_id done_sem;
0029
0030 static rtems_isr done_handler(rtems_vector_number n)
0031 {
0032 rtems_semaphore_release(done_sem);
0033 lm32_interrupt_ack(1 << MM_IRQ_PFPU);
0034 }
0035
0036 rtems_device_driver pfpu_initialize(
0037 rtems_device_major_number major,
0038 rtems_device_minor_number minor,
0039 void *arg
0040 )
0041 {
0042 rtems_status_code sc;
0043 rtems_isr_entry dummy;
0044
0045 sc = rtems_io_register_name(DEVICE_NAME, major, 0);
0046 RTEMS_CHECK_SC(sc, "create PFPU device");
0047
0048 sc = rtems_semaphore_create(
0049 rtems_build_name('P', 'F', 'P', 'U'),
0050 0,
0051 RTEMS_SIMPLE_BINARY_SEMAPHORE,
0052 0,
0053 &done_sem
0054 );
0055 RTEMS_CHECK_SC(sc, "create PFPU done semaphore");
0056
0057 rtems_interrupt_catch(done_handler, MM_IRQ_PFPU, &dummy);
0058 bsp_interrupt_vector_enable(MM_IRQ_PFPU);
0059
0060 return RTEMS_SUCCESSFUL;
0061 }
0062
0063 static void load_program(unsigned int *program, int size)
0064 {
0065 int page;
0066 int word;
0067 volatile unsigned int *pfpu_prog = (unsigned int *)MM_PFPU_CODEBASE;
0068
0069 for (page=0;page<(PFPU_PROGSIZE/PFPU_PAGESIZE);page++) {
0070 MM_WRITE(MM_PFPU_CODEPAGE, page);
0071 for (word=0;word<PFPU_PAGESIZE;word++) {
0072 if (size == 0) return;
0073 pfpu_prog[word] = *program;
0074 program++;
0075 size--;
0076 }
0077 }
0078 }
0079
0080 static void load_registers(float *registers)
0081 {
0082 volatile float *pfpu_regs = (float *)MM_PFPU_DREGBASE;
0083 int i;
0084
0085 for (i=PFPU_SPREG_COUNT;i<PFPU_REG_COUNT;i++)
0086 pfpu_regs[i] = registers[i];
0087 }
0088
0089 static void update_registers(float *registers)
0090 {
0091 volatile float *pfpu_regs = (float *)MM_PFPU_DREGBASE;
0092 int i;
0093
0094 for (i=PFPU_SPREG_COUNT;i<PFPU_REG_COUNT;i++)
0095 registers[i] = pfpu_regs[i];
0096 }
0097
0098 static rtems_status_code pfpu_execute(struct pfpu_td *td)
0099 {
0100 rtems_status_code sc;
0101
0102 load_program(td->program, td->progsize);
0103 load_registers(td->registers);
0104 MM_WRITE(MM_PFPU_MESHBASE, (unsigned int)td->output);
0105 MM_WRITE(MM_PFPU_HMESHLAST, td->hmeshlast);
0106 MM_WRITE(MM_PFPU_VMESHLAST, td->vmeshlast);
0107 MM_WRITE(MM_PFPU_CTL, PFPU_CTL_START);
0108
0109 sc = rtems_semaphore_obtain(done_sem, RTEMS_WAIT, 10);
0110 if (sc != RTEMS_SUCCESSFUL)
0111 return sc;
0112
0113 if (td->update) {
0114 update_registers(td->registers);
0115 if (td->invalidate) {
0116 __asm__ volatile(
0117 "wcsr DCC, r0\n"
0118 "nop\n"
0119 );
0120 }
0121 }
0122
0123 return RTEMS_SUCCESSFUL;
0124 }
0125
0126 rtems_device_driver pfpu_control(
0127 rtems_device_major_number major,
0128 rtems_device_minor_number minor,
0129 void *arg
0130 )
0131 {
0132 rtems_libio_ioctl_args_t *args = arg;
0133
0134 args->ioctl_return = -1;
0135 if (args->command != PFPU_EXECUTE)
0136 return RTEMS_UNSATISFIED;
0137
0138 if (pfpu_execute((struct pfpu_td *)args->buffer) != RTEMS_SUCCESSFUL)
0139 return RTEMS_UNSATISFIED;
0140
0141 args->ioctl_return = 0;
0142 return RTEMS_SUCCESSFUL;
0143 }