restore RemoveCallback3 as RemoveCallbackUsr to get (un)interest working again

callback.c

@@ -271,6 +271,36 @@ int RemoveCallback2(pICallBack self, void *pUserData)
   return 1;
 }
 
+/*--------------------------------------------------------------------------*/
+/*
+ * Remove callback in context -
+ * if the callback function matches and the user function returns zero
+ */
+int RemoveCallbackUsr(pICallBack self, SICSCallBack pFunc, int (*userfunc)(const void* pContext, const void* pUserData), void *pCtx)
+{
+  pCallBackItem pItem;
+
+  if (!CheckPointer(self))
+  {
+    return 0;
+  }
+
+  pItem = self->head;
+  while (pItem != NULL) {
+    if (pItem->pFunc == pFunc) {
+      if(userfunc(pCtx, pItem->pUserData) == 0) {
+        if (debug) {
+          printf("Killing callback at %p\n", self);
+        }
+        pItem->killFlag = 1;
+      }
+    }
+    pItem = pItem->next;
+  }
+  cleanCallbackList(self);
+  return 1;
+}
+
   /*--------------------------------------------------------------------------*/
 int RemoveCallbackCon(pICallBack self, SConnection * con)
 {

confvirtualmot.c

@@ -36,9 +36,10 @@
 #include "confvirtmot.i"
 
 #define BAD_VALUE (-9999.99)
-extern double DoubleTime(void);
 
 extern char *stptok(char *s, char *t, int len, char *brk);
+extern int CheckMotiMatch(const void* context, const void* pUserData);
+extern double DoubleTime(void);
 
 /*---------------------------------------------------------------------------
  An internal data structure which holds information required to control
@@ -299,13 +300,13 @@ static int InterestCallback(int iEvent, void *pEvent, void *pUser)
   assert(pEvent);
   assert(pUser);
 
+  pEventData = (EventInfo *) pEvent;
+  pInfo = (RegisteredInfo *) pUser;
+
   if (!SCisConnected(pInfo->pCon)) {
     return -1;
   }
 
-  pEventData = (EventInfo *) pEvent;
-  pInfo = (RegisteredInfo *) pUser;
-
   if (pInfo->lastValue != pEventData->fVal) {
     pInfo->lastValue = pEventData->fVal;
     (pInfo->pCon)->conEventType = POSITION;
@@ -713,13 +714,13 @@ int ConfigurableVirtualMotorAction(SConnection * pCon, SicsInterp * pSics,
       }
       pRegInfo->lastValue = value;
 
-      RemoveCallbackCon(self->pCall, pCon);
+      RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon); /* only this one */
       lID = RegisterCallback(self->pCall, MOTDRIVE,
                              InterestCallback, pRegInfo, KillInfo);
       SCSendOK(pCon);
       return 1;
     } else if(strcmp(argv[1],"uninterest") == 0) {
-      RemoveCallbackCon(self->pCall, pCon);
+      RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon);
       SCSendOK(pCon);
       return 1;
     } else {

interface.h

@@ -103,6 +103,7 @@ long RegisterCallback(pICallBack pInterface,
                       void *pUserData, KillFuncIT pKill);
 int RemoveCallback(pICallBack pInterface, long iID);
 int RemoveCallback2(pICallBack pInterface, void *pUserData);
+int RemoveCallbackUsr(pICallBack self, SICSCallBack pFunc, int (*userfunc)(const void* pContext, const void* pUserData), void *pCtx);
 int RemoveCallbackCon(pICallBack pInterface, SConnection * pCon);
 
 int CallbackScript(SConnection * pCon, SicsInterp * pSics, void *pData,

motor.c

@@ -1215,6 +1215,20 @@ static int EndScriptCallback(int iEvent, void *pEvent, void *pUser)
   return iRet;
 }
 
+/*
+ * Context test function for callback removal
+ */
+ int CheckMotiMatch(const void* context, const void* pUserData)
+{
+  pMotInfo pMoti = (pMotInfo) pUserData;
+  SConnection *pCon = (SConnection*) context;
+  if (VerifyConnection(pCon) && VerifyConnection(pMoti->pCon)) {
+    if (pMoti->pCon->ident == pCon->ident)
+      return 0;
+  }
+  return 1;
+}
+
 /*----------------------------------------------------------------------------
  The wrapper function for a motor. Commands currently supported are:
  
@@ -1306,14 +1320,14 @@ int MotorAction(SConnection * pCon, SicsInterp * pSics, void *pData,
     }
     pMoti->lastValue = fValue;
 
-    RemoveCallbackCon(self->pCall, pCon);
+    RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon); /* only this one */
     lID = RegisterCallback(self->pCall, MOTDRIVE, InterestCallback,
                            pMoti, KillInfo);
     DeleteTokenList(pList);
     SCSendOK(pCon);
     return 1;
   } else if (strcmp(pCurrent->text, "uninterest") == 0) {
-    RemoveCallbackCon(self->pCall, pCon);
+    RemoveCallbackUsr(self->pCall, InterestCallback, CheckMotiMatch, pCon); /* only this one */
     SCSendOK(pCon);
     DeleteTokenList(pList);
     return 1;