git.gag.com Git - debian/gnuradio/blob - usrp2/firmware/lib/mdelay.c

   1 /* -*- c -*- */
   2 /*
   3  * Copyright 2007 Free Software Foundation, Inc.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, either version 3 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #include "mdelay.h"
  20 #include "memory_map.h"
  21
  22 // Delay about one millisecond.
  23 //
  24 // Need 33,333 cycles at 33 MHz.
  25 // Each time around the loop is 10 cycles
  26 //
  27
  28 #define LOOPCNT(wb_div) (MASTER_CLK_RATE/(wb_div) / 10000)
  29
  30 inline static void
  31 delay_1ms(int loop_count)
  32 {
  33   int   i;
  34   for (i = 0; i < loop_count; i++){
  35     asm volatile ("or  r0, r0, r0\n\
  36                    or  r0, r0, r0\n\
  37                    or  r0, r0, r0\n\
  38                    or  r0, r0, r0\n\
  39                    or  r0, r0, r0\n\
  40                    or  r0, r0, r0\n\
  41                    or  r0, r0, r0\n");
  42   }
  43 }
  44
  45 // delay about ms milliseconds
  46 void
  47 mdelay(int ms)
  48 {
  49   static int loop_count = -1;
  50
  51   if (hwconfig_simulation_p())
  52     return;
  53
  54   if (loop_count < 0){
  55     // set correct loop_count
  56     static unsigned short lc[8] = {
  57       0,
  58       LOOPCNT(1),
  59       LOOPCNT(2),
  60       LOOPCNT(3),
  61       LOOPCNT(4),
  62       LOOPCNT(5),
  63       LOOPCNT(6),
  64       LOOPCNT(7)
  65     };
  66
  67     loop_count = lc[hwconfig_wishbone_divisor() & 0x7];
  68   }
  69
  70   int i;
  71   for (i = 0; i < ms; i++)
  72     delay_1ms(loop_count);
  73 }