Back to home page

LXR

 
 

    


File indexing completed on 2025-05-11 08:23:44

0001 /*  pfpu.c
0002  *
0003  *  Milkymist PFPU driver for RTEMS
0004  *
0005  *  The license and distribution terms for this file may be
0006  *  found in the file LICENSE in this distribution or at
0007  *  http://www.rtems.org/license/LICENSE.
0008  *
0009  *  COPYRIGHT (c) 2010, 2011 Sebastien Bourdeauducq
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( /* Invalidate Level-1 data cache */
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 }