/**************************************************************************/
/* Discription: 							  */
/*  The exec.library SetFunction function is very unsafe to use when	  */
/*  more than one program changes a function in a library.		  */
/*									  */
/*  The programs must change the function back in reverse order.	  */
/*									  */
/*  example:								  */
/*   Two different programs (A & B) changes the DisplayBeep function      */
/*   in intuition.library.						  */
/*									  */
/*-------------  First both programs changes the function ----------------*/
/*   A: A_OldFunc = SetFunction(IntuitionBase,_LVODisplayBeep,A_DispBeep);*/
/*	 A_OldFunc will contain the original position of DisplayBeep.	  */
/*									  */
/*   B: B_OldFunc SetFunction(IntuitionBase,_LVODisplayBeep,B_DispBeep);  */
/*	 B_OldFunc will contain the position of A_DispBeep.		  */
/*									  */
/*----	Then both programs restores the function (In wrong order)---------*/
/*   A: SetFunction(IntuitionBase, _LVODisplayBeep; A_OldFunc)            */
/*	 The original DisplayBeep function will be restored.		  */
/*	 (B_DispBeep will not be called)                                  */
/*									  */
/*   B: SetFunction(IntuitionBase, _LVODisplayBeep; B_OldFunc)            */
/*	 The function A_DispBeep will be started again. 		  */
/*	 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~			  */
/*	 (If program A is no longer present the guru comes when           */
/*	  someone calls the DisplayBeep function.)			  */
/*									  */
/*  This program fixes these problems in a fully tranparent way 	  */
/*									  */
/**************************************************************************/


/**************************************************************************/
/*  LibHeader								  */
/*     NextLib:  Next library in a linked list. 			  */
/*     LibBase:  Pointer to the libraries base (e.g. intuitionbase).      */
/*     Patches:  Pointer to linked list of patches made to this library.  */
/**************************************************************************/
struct LibHeader {
  struct Library    *LibBase;
  struct Patches    *Patches;
  struct LibHeader  *NextLib;
};

/************************************************************************/
/*  Patches								*/
/*     JMP	: Opcode for JMP (used to call the old function).       */
/*     OldFunc	: Address to the previus function (before SetFunction). */
/*     NewFunc	: The new function.					*/
/*     Offset	: Offset in library (Must be negativ).                  */
/*     NextPatch: Pointer to next patch in list.			*/
/*									*/
/*  The first Patch in the library list is just a dummy,		*/
/*  Offset = MAGIC_OFFSET (makes the code more simple).                 */
/*  After the dummy comes all patches made to this library sorted after */
/*  their Offset (lowest value first). If there are more then one patch */
/*  at one offset, the most recent comes first. 			*/
/************************************************************************/
struct Patches {
  USHORT	  JMP;
  LONG		  OldFunc;
  LONG		  NewFunc;
  SHORT 	  Offset;
  struct Patches *NextPatch;
};
/******* Some useful defines ********/
  /* This offset identifies our dummy patch */
#define MAGIC_OFFSET	  123
  /* Kickstart library versions */
#define KS13  35
  /*************/
#define AND &&
#define OR ||

/************* Functions ****************/
       /*			A0		    A1		     D0 	*/
STATIC LONG  NewSetFunction(APTR Offset, struct Library *Library, LONG NewFunc);
extern LONG  OldSetFunction(APTR Offset, struct Library *Library, LONG NewFunc);

       struct LibHeader *MakeHead(struct Library *Library);
STATIC struct Patches	*MakePatch(APTR Offset, struct Patches *Next, LONG NewFunc);

/************* Global variables **********/
extern struct DosBase *DOSBase; 		 /* Must be first */
extern struct SignalSemaphore SetFuncSemaphore;
extern struct LibHeader *MainHeader;

extern __fptr  OldSetFuncPtr;

/*************************************************/
/* NewSetFunction: Replaces the exec SetFunction */
/*************************************************/
LONG NewSetFunction(APTR Offset, struct Library *Library, LONG NewFunc)         /* D0 */
{
  struct LibHeader *HeadPtr, *HeadPtr1;
  struct Patches *Patch;
  LONG	OldFunc = NULL;

  geta4();

  /* old dos.library can't be handled here */
  if (Library == DOSBase  AND  Library->lib_Version <= KS13)
    return OldSetFunction(Offset, Library, NewFunc);

  /* Be sure no one else is using the patch list */
  ObtainSemaphore(&SetFuncSemaphore);

  HeadPtr = MainHeader;

  while (HeadPtr1 = HeadPtr->NextLib) {
    /************************/
    /* Find the library     */
    /************************/

    if (HeadPtr1->LibBase == Library) {
      Patch = HeadPtr1->Patches;

      FOREVER { /* We leave this loop with goto */
	/*******************************************/
	/* Look through the sorted list of patches */
	/*******************************************/
	struct Patches *Patch1 = Patch->NextPatch;

	if (Patch1 == NULL OR Patch1->Offset < (WORD)Offset) {
	  /****************************************/
	  /* No patch at this offset is installed */
	  /* Make a new one.			  */
	  /****************************************/
	  Patch1 = MakePatch(Offset, Patch1, NewFunc);
	  if (Patch1)
	    if (Patch1->OldFunc = OldSetFunction(Offset, Library, NewFunc))
	      OldFunc = (LONG)(Patch->NextPatch = Patch1);
	    else /* SetFunction failed */
	      FreeMem(Patch1,sizeof(struct Patches));
	  goto Done;		    /******************************/
	}

	if (Patch1->Offset == (WORD)Offset) {
	  /***************************************/
	  /* There are patches already installed */
	  /* Find out if anyone is to be removed */
	  /***************************************/
	  struct Patches *Patch2;

	  /********************************************************/
	  /* Some tasks don't care about the data returned from   */
	  /* SetFunction (e.g. Xoper), instead they look directly */
	  /* into library structure to find the old function.	  */
	  /* Since this is bad programming we can't do much about */
	  /* it.  If we are lucky no other patches are made to	  */
	  /* the same offset.					  */
	  /********************************************************/
	  if ((LONG)Patch1 == NewFunc OR Patch1->OldFunc == NewFunc) {
	    /*******************************************************/
	    /* User want's to remove the last (or only) made patch */
	    /* This is easy, just remove it			   */
	    /*******************************************************/
	    OldFunc = OldSetFunction(Offset, Library, Patch1->OldFunc);
	    if (OldFunc == Patch1->NewFunc) {
	      Patch->NextPatch = Patch1->NextPatch;
	      FreeMem(Patch1,sizeof(struct Patches));
	    }
	    else {
	      /******************************************************/
	      /* If we gets here something is very wrong	    */
	      /* Either has someone managed to change the vector    */
	      /* from the outside (Virus !?) or SetFunction failed  */
	      /* for some strange reason.			    */
	      /* We just restore the vector and hopes for the best. */
	      /******************************************************/
	      if (OldFunc) {
		OldSetFunction(Offset, Library, OldFunc);
		OldFunc = NULL;
	      }
	      goto Done;
	    }

	    /**************************************************/
	    /* If this was the last patch made to the library */
	    /* Release unused memory.			      */
	    /**************************************************/
	    if (Patch->Offset == MAGIC_OFFSET AND
		Patch->NextPatch == NULL) {
	      HeadPtr->NextLib = HeadPtr1->NextLib;
	      FreeMem(Patch,sizeof(struct Patches));
	      FreeMem(HeadPtr1,sizeof(struct LibHeader));
	    }
	    goto Done;		    /******************************/
	  }

	  /********************************************************/
	  /* Are there any more patches made to the same offset ? */
	  /********************************************************/
	  while ((Patch2 = Patch1->NextPatch)  AND
		 Patch2->Offset == (WORD)Offset) {
	    if ((LONG)Patch2 == NewFunc OR Patch2->OldFunc == NewFunc) {
	      /*****************************************************/
	      /* The user want's to remove an older patched        */
	      /* This is more tricky, must restore OldFunc pointer */
	      /*****************************************************/
	      Patch1->NextPatch = Patch2->NextPatch;
	      Patch1->OldFunc = Patch2->OldFunc;
	      OldFunc = Patch2->NewFunc;  /* No SetFunction here !! */
	      FreeMem(Patch2,sizeof(struct Patches));
	      goto Done;	    /******************************/
	    }
	    Patch1 = Patch2;
	  }
	  /*****************************************/
	  /* This was a new patch which takes over */
	  /*****************************************/
	  Patch1 = MakePatch(Offset, Patch->NextPatch, NewFunc);
	  if (Patch1)
	    if (Patch1->OldFunc = OldSetFunction(Offset, Library, NewFunc))
	      OldFunc = (LONG)(Patch->NextPatch = Patch1);
	    else  /* SetFunction failed */
	      FreeMem(Patch1,sizeof(struct Patches));
	  goto Done;		    /******************************/
	} /* if (Patch1->Offset == ... */
	/* Try next patch in list */
	Patch = Patch1;
      } /* FOREVER */
    } /* if (HeadPtr->Lib ... */
    /* try next library in list */
    HeadPtr = HeadPtr1;
  } /* while (HeadPtr1 = ... */

  /***************/
  /* New library */
  /***************/
  HeadPtr1 = HeadPtr->NextLib = MakeHead(Library);

  /*******************/
  /* Crete the patch */
  /*******************/
  if (HeadPtr1) {
    if (Patch = MakePatch(Offset, NULL, NewFunc)) {
      /* crete a dummy patch */
      if (HeadPtr1->Patches = MakePatch((APTR)MAGIC_OFFSET, Patch, NULL)) {
	if (Patch->OldFunc = OldSetFunction(Offset, Library, NewFunc)) {
	  HeadPtr->NextLib = HeadPtr1;
	  OldFunc = (LONG)Patch;
	  goto Done;
	} /* SetFunction failed */
	FreeMem(HeadPtr1->Patches,sizeof(struct Patches));
      } /* Can't crete dummy patch */
      FreeMem(Patch,sizeof(struct Patches));
    } /* Can't crete real patch */
    FreeMem(HeadPtr1,sizeof(struct LibHeader));
  } /* Can't crete header */

Done:
  ReleaseSemaphore(&SetFuncSemaphore);

  return OldFunc;
}

struct LibHeader *MakeHead(struct Library *Library)
{
  struct LibHeader *Head;

  if (Head = AllocMem(sizeof(struct LibHeader),MEMF_CLEAR))
    Head->LibBase = Library;
  return Head;
}

struct Patches *MakePatch(APTR Offset, struct Patches *Next, LONG NewFunc)
{
  struct Patches *Patch;

  if (Patch = AllocMem(sizeof(struct Patches),MEMF_CLEAR)) {
    Patch->JMP = 0x4ef9; /* JMP opcode */
    Patch->NewFunc = NewFunc;
    Patch->Offset = (WORD)Offset;
    Patch->NextPatch = Next;
  }
  return Patch;
}