arch/arm-native/soc/broadcom/283x/vcmbox/vcmbox_init.c

   1 /*
   2     Copyright © 2013, The AROS Development Team. All rights reserved.
   3     $Id$
   4 */
   5
   6 #define DEBUG 0
   7
   8 #include <aros/debug.h>
   9 #include <aros/symbolsets.h>
  10 #include <aros/libcall.h>
  11 #include <proto/openfirmware.h>
  12 #include <proto/kernel.h>
  13 #include <proto/exec.h>
  14 #include <proto/vcmbox.h>
  15
  16 #include <asm/bcm2835.h>
  17 #include <hardware/videocore.h>
  18
  19 #include "vcmbox_private.h"
  20
  21
  22 static int vcmbox_init(struct VCMBoxBase *VCMBoxBase)
  23 {
  24     int retval = TRUE;
  25
  26     D(bug("[VCMBox] vcmbox_init()\n"));
  27
  28     InitSemaphore(&VCMBoxBase->vcmb_Sem);
  29
  30     D(bug("[VCMBox] vcmbox_init: Initialised Semaphore @ 0x%p\n", &VCMBoxBase->vcmb_Sem));
  31
  32     return retval;
  33 }
  34
  35 AROS_LH1(unsigned int, VCMBoxStatus,
  36                 AROS_LHA(void *, mb, A0),
  37                 struct VCMBoxBase *, VCMBoxBase, 1, Vcmbox)
  38 {
  39     AROS_LIBFUNC_INIT
  40
  41     D(bug("[VCMBox] VCMBoxStatus(0x%p)\n", mb));
  42
  43     return *((volatile unsigned int *)(mb + VCMB_STATUS));
  44
  45     AROS_LIBFUNC_EXIT
  46 }
  47
  48 AROS_LH2(volatile unsigned int *, VCMBoxRead,
  49                 AROS_LHA(void *, mb, A0),
  50                 AROS_LHA( unsigned int, chan, D0),
  51                 struct VCMBoxBase *, VCMBoxBase, 2, Vcmbox)
  52 {
  53     AROS_LIBFUNC_INIT
  54
  55     unsigned int try = 0x2000000;
  56     unsigned int msg;
  57
  58     D(bug("[VCMBox] VCMBoxRead(chan %d @ 0x%p)\n", chan, mb));
  59
  60     if (chan <= VCMB_CHAN_MAX)
  61     {
  62         while(1)
  63         {
  64             ObtainSemaphore(&VCMBoxBase->vcmb_Sem);
  65             APTR ssp = SuperState();
  66             while ((VCMBoxStatus(mb) & VCMB_STATUS_READREADY) != 0)
  67             {
  68                 asm volatile ("mcr p15, 0, %[r], c7, c14, 0" : : [r] "r" (0) );
  69
  70                 if(try-- == 0)
  71                 {
  72                     break;
  73                 }
  74             }
  75             asm volatile ("mcr p15, 0, %[r], c7, c10, 5" : : [r] "r" (0) );
  76
  77             msg = *((volatile unsigned int *)(mb + VCMB_READ));
  78
  79             asm volatile ("mcr p15, 0, %[r], c7, c10, 5" : : [r] "r" (0) );
  80             UserState(ssp);
  81             ReleaseSemaphore(&VCMBoxBase->vcmb_Sem);
  82
  83             if ((msg & VCMB_CHAN_MASK) == chan)
  84                 return (volatile unsigned int *)(msg & ~VCMB_CHAN_MASK);
  85         }
  86     }
  87     return (volatile unsigned int *)-1;
  88
  89     AROS_LIBFUNC_EXIT
  90 }
  91
  92 AROS_LH3(void, VCMBoxWrite,
  93                 AROS_LHA(void *, mb, A0),
  94                 AROS_LHA( unsigned int, chan, D0),
  95                 AROS_LHA(void *, msg, A1),
  96                 struct VCMBoxBase *, VCMBoxBase, 3, Vcmbox)
  97 {
  98     AROS_LIBFUNC_INIT
  99
 100     D(bug("[VCMB] VCMBWrite(chan %d @ 0x%p, msg @ 0x%p)\n", chan, mb, msg));
 101
 102     if ((((unsigned int)msg & VCMB_CHAN_MASK) == 0) && (chan <= VCMB_CHAN_MAX))
 103     {
 104         ObtainSemaphore(&VCMBoxBase->vcmb_Sem);
 105         APTR ssp = SuperState();
 106         while ((VCMBoxStatus(mb) & VCMB_STATUS_WRITEREADY) != 0)
 107         {
 108             asm volatile ("mcr p15, 0, %[r], c7, c14, 0" : : [r] "r" (0) );
 109         }
 110
 111         asm volatile ("mcr p15, 0, %[r], c7, c10, 5" : : [r] "r" (0) );
 112
 113         *((volatile unsigned int *)(mb + VCMB_WRITE)) = ((unsigned int)msg | chan);
 114         UserState(ssp);
 115         ReleaseSemaphore(&VCMBoxBase->vcmb_Sem);
 116     }
 117
 118     AROS_LIBFUNC_EXIT
 119 }
 120
 121 ADD2INITLIB(vcmbox_init, 0)