Updates for the Raspnerry Pi Compute Module - changes to the gpio program

[wiringPi.git] / wiringPi / softTone.c

diff --git a/wiringPi/softTone.c b/wiringPi/softTone.c
index b4a89f8a250ee8ca803cf20e88b6986937e54087..e2fb73711927adadf33557484ce8b55c84b981f4 100644 (file)
--- a/wiringPi/softTone.c
+++ b/wiringPi/softTone.c
@@ -36,7 +36,8 @@
 
 #define        PULSE_TIME      100
 
-static int freqs [MAX_PINS] ;
+static int freqs         [MAX_PINS] ;
+static pthread_t threads [MAX_PINS] ;
 
 static int newPin = -1 ;
 
@@ -50,6 +51,10 @@ static int newPin = -1 ;
 static PI_THREAD (softToneThread)
 {
   int pin, freq, halfPeriod ;
+  struct sched_param param ;
+
+  param.sched_priority = sched_get_priority_max (SCHED_RR) ;
+  pthread_setschedparam (pthread_self (), SCHED_RR, &param) ;
 
   pin    = newPin ;
   newPin = -1 ;
@@ -105,17 +110,41 @@ void softToneWrite (int pin, int freq)
 int softToneCreate (int pin)
 {
   int res ;
+  pthread_t myThread ;
 
   pinMode      (pin, OUTPUT) ;
   digitalWrite (pin, LOW) ;
 
+  if (threads [pin] != 0)
+    return -1 ;
+
   freqs [pin] = 0 ;
 
   newPin = pin ;
-  res = piThreadCreate (softToneThread) ;
+  res    = pthread_create (&myThread, NULL, softToneThread, NULL) ;
 
   while (newPin != -1)
     delay (1) ;
 
+  threads [pin] = myThread ;
+
   return res ;
 }
+
+
+/*
+ * softToneStop:
+ *     Stop an existing softTone thread
+ *********************************************************************************
+ */
+
+void softToneStop (int pin)
+{
+  if (threads [pin] != 0)
+  {
+    pthread_cancel (threads [pin]) ;
+    pthread_join   (threads [pin], NULL) ;
+    threads [pin] = 0 ;
+    digitalWrite (pin, LOW) ;
+  }
+}