/*
 * Baker utilization test.
 *
 * Written by J. Vidal
 * Copyright (C) Jan, 2002 OCERA Consortium.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2.
 *
 * StandAlone RTLinux integration 
 * written by Vicente Esteve LLoret <viesllo@inf.upv.es> 
 * Copyright (C) Feb, 2003 OCERA Consortium.
 *
 * 
 */


#include <rtl_sched.h>
#include <rtl_debug.h>
#include <signal.h>
#include <time.h>

#define ONEMILI ((hrtime_t) 1000*1000)
#define ONEMICRO ((hrtime_t) 1000)
#define INCR 1000 //1 ns.
#define NTASKS 6
#define BASEPERIOD ((hrtime_t) 3125000) //period in ns. 3125000 => 320 Hz
#define MAXLOOPS 10000000
#define NANOSXMILIS (1*1000) // us
#define REFERENCE NANOSXMILIS
#define MYMHZ 665LL



struct fixed_th {
  long long period,resume_time;
  unsigned long load;
  int id;
};

//#define __FIXED_DEBUG__
extern hrtime_t start_time, LOOPS_X_MILI;
extern int found;
hrtime_t LOOPS_X_MILI;

void *regular_thread_code(void *arg);
void *baker_test_code(void *arg);


static inline void waste_time(unsigned long iters,hrtime_t *time){
  hrtime_t t1,t2;

  __asm volatile ("cpuid; rdtsc" : "=A"(t1): :"ebx","ecx");

{unsigned long i; for (i=0;i<iters;i++);}

  __asm volatile ("cpuid; rdtsc" : "=A"(t2): :"ebx","ecx");

  *time=t2-t1;
  *time=((*time/MYMHZ)); /*time in nanos*/
}



static inline void eat(unsigned long howmuch){
  unsigned int loops;
  hrtime_t t;

  t=((howmuch)*LOOPS_X_MILI);
//  loops=t/ONEMILI;
  loops=t/ONEMICRO;
  waste_time(loops,&t);
}

static inline void measure(unsigned long iters,hrtime_t *time){
  hrtime_t t1,t2;
  char texto[20];

  __asm volatile ("cpuid; rdtsc" : "=A"(t1): :"ebx","ecx");
// Need to include the cost of two long long operations.
// WHY????
  t2=(*time)*t1;
  (*time)/=(t2+1);
  
   {unsigned long i; for (i=0;i<iters;i++);}

  __asm volatile ("cpuid; rdtsc" : "=A"(t2): :"ebx","ecx");
  *time=t2-t1;

  hexatoascii((unsigned long) *time,texto);
  PutString(12,12,texto,0x25);
 
  hexatoascii((unsigned long) (*time>>32),texto);
  PutString(13,12,texto,0x25);
 
  *time=((*time/MYMHZ)); /*time in nanos*/

  hexatoascii((unsigned long) *time,texto);
  PutString(15,12,texto,0x25);
  hexatoascii((unsigned long) (*time>>32),texto);
  PutString(16,12,texto,0x25);
 
}


static inline void regulator(hrtime_t *nops_x_mili, hrtime_t period){
  hrtime_t nanos,error,last_error=REFERENCE,action;
  unsigned int i=0;
  char texto[20];
  char texto2[20];
  *nops_x_mili=REFERENCE/10;

  for (i=0; i<MAXLOOPS;i++){
    hexatoascii((unsigned long) i,texto2);
    PutString(9,40,texto2,0x50);
    
    measure(*nops_x_mili,&nanos);
    error=(REFERENCE-nanos);
    hexatoascii((unsigned long) error,texto);
    PutString(9,10,texto,0x20);
    hexatoascii((unsigned long) (error>>32),texto);
    PutString(10,10,texto,0x20);
   
    if (!error){
      found=1;
      break; 
    }

    action = error/2; /*+ ((error-last_error)/period)/1 
	     + ((error+last_error)/2)/1;*/
       
    (*nops_x_mili) += action;
	
//    if (*nops_x_mili > 50*REFERENCE) *nops_x_mili=50*REFERENCE;
    
    last_error = error;
//    pthread_wait_np ();
  }

  return;
}