wpa_supplicant_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <boost/thread.hpp>
00003 #include <actionlib/server/action_server.h>
00004 #include <actionlib_msgs/GoalStatus.h>
00005 #include <dynamic_reconfigure/server.h>
00006 #include <unistd.h>
00007 #include <fcntl.h>
00008 #include <string.h>
00009 #include <queue>
00010 #include <algorithm>                  
00011 #include <wpa_supplicant_node/ScanAction.h>
00012 #include <wpa_supplicant_node/AssociateAction.h>
00013 #include <wpa_supplicant_node/AddNetwork.h>
00014 #include <wpa_supplicant_node/RemoveNetwork.h>
00015 #include <wpa_supplicant_node/SetNetworkState.h>
00016 #include <wpa_supplicant_node/SetNetworkParameters.h>
00017 #include <wpa_supplicant_node/NetworkList.h>
00018 #include <wpa_supplicant_node/FrequencyList.h>
00019 #include <wpa_supplicant_node/SecurityProperties.h>
00020 #include <wpa_supplicant_node/WpaSupplicantNodeConfig.h>
00021 
00022 extern "C" {
00023 #include <includes.h>
00024 #include <common.h>
00025 #include <wpa_supplicant_i.h>
00026 #include <driver_i.h>
00027 #include <eloop.h>
00028 #include <drivers/driver.h>
00029 #include <scan.h>
00030 #include <bss.h>
00031 #include <config.h>
00032 #include <common/wpa_common.h>
00033 #include <common/ieee802_11_defs.h>
00034 #include "wpa_supplicant_node.h"
00035 #include <rsn_supp/wpa.h>
00036 }
00037 
00038 typedef boost::function<void ()> WorkFunction;
00039 
00040 void reconfigure(wpa_supplicant_node::WpaSupplicantNodeConfig &config, unsigned int& level);
00041 
00042 static class RosApi {
00043   //int eloop_pid_;
00044   bool initialized_;
00045   std::queue<WorkFunction> work_queue_;
00046   boost::condition_variable main_thread_cv_;
00047   enum main_thread_states { MAIN_THREAD_NOT_WAITING, MAIN_THREAD_WAITING, MAIN_THREAD_PAUSED };
00048   volatile main_thread_states main_thread_state_;
00049   int pipefd[2];
00050   boost::shared_ptr<boost::thread> ros_spin_loop_;
00051   volatile bool shutting_down_;
00052 
00053 private:
00054   bool first_call_to_reconfigure;
00055 
00056 public:
00057   boost::mutex mutex_;
00058   
00059   RosApi()
00060   {
00061     shutting_down_ = false;
00062     main_thread_state_ = MAIN_THREAD_NOT_WAITING;
00063   }
00064 
00065   void doWork()
00066   {
00067     char buffer[16]; // Any non-zero size is fine here.
00068 
00069     ROS_INFO("doWork()");
00070     
00071     {
00072       boost::mutex::scoped_lock lock(mutex_);
00073       mainThreadWait(lock);
00074     }
00075 
00076     int bytes;
00077     do {
00078       bytes = read(pipefd[0], buffer, sizeof(buffer));
00079       ROS_INFO("doWork read %i bytes from the dummy fifo.", bytes);
00080     } while (bytes == sizeof(buffer));
00081 
00082     while (1)
00083     { 
00084       WorkFunction work;
00085       {
00086         boost::mutex::scoped_lock lock(mutex_);
00087         if (work_queue_.empty())
00088           break;
00089         work = work_queue_.front();
00090         work_queue_.pop();
00091       }
00092       work();
00093     }
00094   }
00095 
00096   void addWork(const WorkFunction &f)
00097   {
00098     ROS_INFO("addWork()");
00099     
00100     boost::mutex::scoped_lock lock(mutex_);
00101 
00102     work_queue_.push(f);
00103     triggerWork();
00104   }     
00105 
00106   void triggerWork()
00107   {
00108     if (write(pipefd[1], pipefd, 1) != 1) // pipefd is used as a dummy buffer.
00109       ROS_ERROR("addWork Failed to write to wakeup pipe.");
00110   }
00111 
00112   int init(int *argc, char ***argv)
00113   {
00114     ROS_INFO("ros_init");
00115 
00116     // Register the socket that will be used to wake up the event loop.
00117     if (pipe2(pipefd, O_NONBLOCK | O_CLOEXEC))
00118     {
00119       ROS_FATAL("pipe2 failed: %s (%i)", strerror(errno), errno);
00120       return -1;
00121     }
00122     ROS_INFO("pipefds: %i <- %i", pipefd[0], pipefd[1]);
00123     
00124     ros::init(*argc, *argv, "wpa_supplicant", ros::init_options::NoSigintHandler);
00125 
00126     static dynamic_reconfigure::Server<wpa_supplicant_node::WpaSupplicantNodeConfig> srv;
00127     static dynamic_reconfigure::Server<wpa_supplicant_node::WpaSupplicantNodeConfig>::CallbackType f;
00128     f = boost::bind(&RosApi::reconfigure, this, _1, _2);
00129 
00130     first_call_to_reconfigure = true;
00131     srv.setCallback(f);
00132     first_call_to_reconfigure = false;
00133     
00134     ros_spin_loop_.reset(new boost::thread(boost::bind(&ros::spin)));
00135 
00136     return 0;
00137   }
00138 
00139 private:
00140   void reconfigure(wpa_supplicant_node::WpaSupplicantNodeConfig &config, unsigned int& level);
00141 
00142   void waitForMainThreadState(boost::mutex::scoped_lock &lock, main_thread_states target)
00143   {
00144     while (main_thread_state_ != target && !shutting_down_)
00145     {
00146       main_thread_cv_.wait(lock);
00147     }
00148   }
00149 
00150   void mainThreadWait(boost::mutex::scoped_lock &lock)
00151   { // If somebody waiting for us, let them make progress.
00152     if (main_thread_state_ == MAIN_THREAD_WAITING)
00153     {
00154       main_thread_state_ = MAIN_THREAD_PAUSED;
00155       main_thread_cv_.notify_all();
00156       waitForMainThreadState(lock, MAIN_THREAD_NOT_WAITING);
00157     }
00158   }
00159 
00160 public:
00161   bool waitForMainThread(boost::mutex::scoped_lock &lock)
00162   { // Call with mutex_ held. Returns with main thread waiting.
00163     main_thread_state_ = MAIN_THREAD_WAITING;
00164     triggerWork();
00165     waitForMainThreadState(lock, MAIN_THREAD_PAUSED);
00166     main_thread_state_ = MAIN_THREAD_NOT_WAITING;
00167     main_thread_cv_.notify_all();
00168     return !shutting_down_;
00169   }
00170   
00171   void init2()
00172   {
00173     ROS_INFO("ros_init");
00174     eloop_register_read_sock(pipefd[0], &ros_do_work, NULL, NULL);
00175   }
00176 
00177   void uninit()
00178   {
00179     ROS_INFO("ros_deinit");
00180   
00181     boost::mutex::scoped_lock lock(mutex_);
00182     shutting_down_ = true;
00183     mainThreadWait(lock);
00184 
00185     ros::shutdown(); 
00186     // FIXME Need to wait for shutdown to complete?
00187   
00188     if (ros_spin_loop_ && !ros_spin_loop_->timed_join((boost::posix_time::milliseconds) 2000))
00189       ROS_ERROR("ROS thread did not die after two seconds. Exiting anyways. This is probably a bad sign.");
00190 
00191     eloop_unregister_read_sock(pipefd[0]);
00192     close(pipefd[0]);
00193     close(pipefd[1]);
00194   }
00195 } g_ros_api;
00196 
00197 typedef actionlib::ActionServer<wpa_supplicant_node::ScanAction> ScanActionServer;
00198 typedef actionlib::ActionServer<wpa_supplicant_node::AssociateAction> AssociateActionServer;
00199 const ScanActionServer     ::GoalHandle null_scan_goal_handle_;
00200 const AssociateActionServer::GoalHandle null_associate_goal_handle_;
00201 
00202 struct ros_interface
00203 {
00204   static char country_code[2];
00205   static struct wpa_global *global;
00206 
00207 private:
00208   wpa_supplicant *wpa_s_;
00209   
00210   ros::NodeHandle nh_;
00211 
00212 //Scan Action
00213   boost::recursive_mutex scan_mutex_;
00214   ScanActionServer sas_;
00215   std::queue<ScanActionServer::GoalHandle> scan_queue_;
00216   ScanActionServer::GoalHandle current_scan_;
00217   std::vector<int> current_scan_frequencies_;
00218 
00219 // Associate Action
00220   boost::recursive_mutex associate_mutex_;
00221   AssociateActionServer aas_;
00222   AssociateActionServer::GoalHandle active_association_;
00223   bool associate_work_requested_;
00224   std::queue<AssociateActionServer::GoalHandle> associate_goal_queue_;
00225   std::queue<AssociateActionServer::GoalHandle> associate_cancel_queue_;
00226 
00227 // Services
00228   ros::ServiceServer add_network_service_;
00229   ros::ServiceServer remove_network_service_;
00230   ros::ServiceServer set_network_state_service_;
00231   ros::ServiceServer set_network_parameter_service_;
00232 
00233 // Topics  
00234   ros::Publisher frequency_list_publisher_;
00235   ros::Publisher network_list_publisher_;
00236   ros::Publisher association_publisher_;
00237   ros::Publisher scan_publisher_;
00238 
00239 public:
00240   ros_interface(const ros::NodeHandle &nh, wpa_supplicant *wpa_s) :
00241     wpa_s_(wpa_s),
00242     nh_(ros::NodeHandle(nh, wpa_s->ifname)),
00243     sas_(nh_, "scan",      boost::bind(&ros_interface::scanGoalCallback,      this, _1), boost::bind(&ros_interface::scanCancelCallback,      this, _1), true),
00244     aas_(nh_, "associate", boost::bind(&ros_interface::associateGoalCallback, this, _1), boost::bind(&ros_interface::associateCancelCallback, this, _1), true)
00245   {
00246     add_network_service_ = nh_.advertiseService("add_network", &ros_interface::addNetwork, this);
00247     remove_network_service_ = nh_.advertiseService("remove_network", &ros_interface::removeNetwork, this);
00248     set_network_state_service_ = nh_.advertiseService("set_network_state", &ros_interface::setNetworkState, this);
00249     set_network_parameter_service_ = nh_.advertiseService("set_network_parameter", &ros_interface::setNetworkParameter, this);
00250 
00251     network_list_publisher_ = nh_.advertise<wpa_supplicant_node::NetworkList>("network_list", 1, true);
00252     frequency_list_publisher_ = nh_.advertise<wpa_supplicant_node::FrequencyList>("frequency_list", 1, true);
00253     association_publisher_ = nh_.advertise<wpa_supplicant_node::AssociateFeedback>("association_state", 1, true);
00254     scan_publisher_ = nh_.advertise<wpa_supplicant_node::ScanResult>("scan_results", 1, true);
00255 
00256     if (!ros_interface::global)
00257       {
00258         ros_interface::global = wpa_s->global;
00259         ROS_INFO("calling setDriverCountry");
00260         setDriverCountry();
00261       }
00262     
00263     eloop_register_timeout(10, 0, delayedPublishFrequencyList, wpa_s, wpa_s);
00264     publishNetworkList();
00265     publishUnassociated();
00266   }
00267 
00268   static void setCountryCode(const char *country_code)
00269   {
00270     if (strlen(country_code) != 2)
00271       {
00272         ROS_ERROR("Invalid country code %s", country_code);
00273         return;
00274       }
00275 
00276     ros_interface::country_code[0] = country_code[0];
00277     ros_interface::country_code[1] = country_code[1];
00278     
00279     if (global && global->ifaces && global->ifaces->ros_api)
00280       {
00281         global->ifaces->ros_api->setDriverCountry();
00282         eloop_register_timeout(0, 100000, delayedPublishFrequencyList, global->ifaces, NULL);
00283       }
00284   }
00285 
00286   void publishUnassociated()
00287   {
00288     wpa_supplicant_node::AssociateFeedback fbk;
00289     association_publisher_.publish(fbk);
00290   }
00291 
00292   void ifaceIdle()
00293   {
00294   }
00295 
00296   void scanCompleted(wpa_scan_results *scan_res)
00297   {
00298     ROS_INFO("scanCompleted");
00299 
00300     boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00301     
00302     eloop_cancel_timeout(scanTimeoutHandler, wpa_s_, NULL);
00303     
00304     wpa_supplicant_node::ScanResult rslt;
00305     if (scan_res)
00306     {
00307       fillRosResp(rslt, *scan_res);
00308       scan_publisher_.publish(rslt);
00309     }
00310 
00311     if (current_scan_ == null_scan_goal_handle_)
00312       ROS_ERROR("scanCompleted with current_scan_ not set.");
00313     else
00314     {
00315       if (scan_res)
00316       {
00317         // TODO copy output to response.
00318         ROS_INFO("Scan completed successfully.");
00319         current_scan_.setSucceeded(rslt);
00320       }
00321       else
00322       {
00323         ROS_INFO("Scan failed.");
00324         current_scan_.setAborted(rslt);
00325       }
00326       current_scan_ = null_scan_goal_handle_;
00327       lockedScanTryActivate();
00328     }
00329   }
00330 
00331   void assocFailed(const u8 *bssid, const char *s)
00332   {
00333     ROS_INFO("assocFailed");
00334     boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00335     
00336     eloop_cancel_timeout(associateTimeoutHandler, wpa_s_, NULL);
00337     
00338     if (active_association_ == null_associate_goal_handle_)
00339     {
00340       ROS_ERROR("Got disassociation with no active association on BSSID: " MACSTR " (%s)", MAC2STR(bssid), s);
00341     }
00342     else
00343     {
00344       if (os_memcmp(bssid, &active_association_.getGoal()->bssid, ETH_ALEN) &&
00345           !is_zero_ether_addr(bssid))
00346       {
00347         ROS_ERROR("Got disassociation on unexpected BSSID: " MACSTR " (%s)", MAC2STR(bssid), s);
00348       }
00349     
00350       active_association_.setAborted();
00351       active_association_ = null_associate_goal_handle_;
00352     }
00353 
00354     publishUnassociated();
00355     requestAssociateWork();
00356   }
00357   
00358   void assocSucceeded()
00359   {
00360     ROS_INFO("assocSucceeded");
00361     boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00362     
00363     eloop_cancel_timeout(associateTimeoutHandler, wpa_s_, NULL);
00364 
00365     wpa_supplicant_node::AssociateFeedback fbk;
00366     fbk.associated = true;
00367     fillRosBss(fbk.bss, *wpa_s_->current_bss);
00368     // FIXME Set BSS.
00369     // FIXME Flag a problem if called twice.
00370     active_association_.publishFeedback(fbk);
00371     association_publisher_.publish(fbk);
00372   }
00373 
00374   void publishNetworkList()
00375   {
00376     wpa_supplicant_node::NetworkList netlist;                          
00377 
00378     for (wpa_ssid *ssid = wpa_s_->conf->ssid; ssid; ssid = ssid->next)
00379     { 
00380       wpa_supplicant_node::Network net;
00381       net.network_id = ssid->id;
00382       net.enabled = !ssid->disabled;
00383 
00384       char **parameters = wpa_config_get_all(ssid, 0);
00385       for (char **cur_parameter = parameters; *cur_parameter; cur_parameter += 2)
00386       {
00387         wpa_supplicant_node::NetworkParameter param;
00388         param.key = cur_parameter[0];
00389         param.value = cur_parameter[1];
00390         net.parameters.push_back(param);
00391         free(cur_parameter[0]);
00392         free(cur_parameter[1]);
00393       }
00394       free(parameters);
00395 
00396       netlist.networks.push_back(net);
00397     }
00398 
00399     network_list_publisher_.publish(netlist);
00400   }
00401 
00402 private:
00403   void setDriverCountry()
00404   {
00405     if (!ros_interface::country_code[0])
00406       return;
00407 
00408     wpa_s_->conf->country[0] = ros_interface::country_code[0];
00409     wpa_s_->conf->country[1] = ros_interface::country_code[1];
00410 
00411     if (wpa_drv_set_country(wpa_s_, wpa_s_->conf->country))
00412       {
00413         ROS_ERROR("Failed to set driver country code %s", ros_interface::country_code);
00414         return;
00415       }
00416 
00417     struct wpa_supplicant *wpa_s_iter = wpa_s_->next;
00418     
00419     while (wpa_s_iter)
00420       {
00421         if (wpa_s_iter->ros_api)
00422           eloop_register_timeout(0, 100000, delayedPublishFrequencyList, wpa_s_iter, NULL);
00423         wpa_s_iter = wpa_s_iter->next;
00424       }
00425   }
00426 
00427   static void delayedPublishFrequencyList(void *wpa_s, void *repeat_p)
00428   {
00429     ((wpa_supplicant *) wpa_s)->ros_api->publishFrequencyList();
00430 
00431     if (repeat_p != NULL)
00432       eloop_register_timeout(10, 0, delayedPublishFrequencyList, wpa_s, repeat_p);
00433   }
00434 
00435   void publishFrequencyList()
00436   {
00437     wpa_supplicant_node::FrequencyList f;
00438 
00439     uint16_t num_modes, flags;
00440     struct hostapd_hw_modes *modes; 
00441 
00442     modes = wpa_drv_get_hw_feature_data(wpa_s_, &num_modes, &flags);
00443     
00444     if (modes != NULL)
00445       {
00446         for (struct hostapd_hw_modes *mode = modes; mode < modes + num_modes; mode++)
00447           {
00448             if (mode->channels)
00449               {
00450                 for (struct hostapd_channel_data *channel = mode->channels; 
00451                      channel < mode->channels + mode->num_channels;
00452                      channel++)
00453                   {
00454                     if (!(channel->flag & HOSTAPD_CHAN_DISABLED) && 
00455                         std::find(f.frequencies.begin(), f.frequencies.end(), channel->freq) == f.frequencies.end())
00456                       {
00457                         f.frequencies.push_back(channel->freq);
00458                       }
00459                   }
00460                 free(mode->channels);
00461               }
00462             if (mode->rates)
00463               free(mode->rates);
00464           }
00465         free(modes);
00466       }
00467     else
00468       {
00469         for (int i = 2412; i <= 2462; i += 5)
00470           f.frequencies.push_back(i);
00471         for (int i = 5180; i <= 5320; i += 20)
00472           f.frequencies.push_back(i);
00473         for (int i = 5500; i <= 5580; i += 20)
00474           f.frequencies.push_back(i);
00475         for (int i = 5500; i <= 5580; i += 20)
00476           f.frequencies.push_back(i);
00477         for (int i = 5680; i <= 5700; i += 20)
00478           f.frequencies.push_back(i);
00479         for (int i = 5745; i <= 5825; i += 20)
00480           f.frequencies.push_back(i);
00481       }
00482 
00483     frequency_list_publisher_.publish(f);
00484   }
00485 
00486   bool addNetwork(wpa_supplicant_node::AddNetwork::Request &req, wpa_supplicant_node::AddNetwork::Response &rsp)
00487   {
00488     ROS_INFO("addNetwork");
00489     
00490     boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00491     if (!g_ros_api.waitForMainThread(lock))
00492       return false;
00493   
00494 
00495     return true;
00496   }
00497 
00498   bool removeNetwork(wpa_supplicant_node::RemoveNetwork::Request &req, wpa_supplicant_node::RemoveNetwork::Response &rsp)
00499   {
00500     ROS_INFO("removeNetwork");
00501     
00502     boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00503     if (!g_ros_api.waitForMainThread(lock))
00504       return false;
00505     
00506     
00507     return true;
00508   }
00509 
00510   bool setNetworkState(wpa_supplicant_node::SetNetworkState::Request &req, wpa_supplicant_node::SetNetworkState::Response &rsp)
00511   {
00512     ROS_INFO("setNetworkState");
00513     
00514     boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00515     if (!g_ros_api.waitForMainThread(lock))
00516       return false;
00517     
00518 
00519     return true;
00520   }
00521 
00522   bool setNetworkParameter(wpa_supplicant_node::SetNetworkParameters::Request &req, wpa_supplicant_node::SetNetworkParameters::Response &rsp)
00523   {
00524     ROS_INFO("setNetworkParameter");
00525     
00526     boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00527     if (!g_ros_api.waitForMainThread(lock))
00528       return false;
00529     
00530 
00531     return true;
00532   }
00533   void requestAssociateWork()
00534   {
00535       associate_work_requested_ = true;
00536       g_ros_api.addWork(boost::bind(&ros_interface::associateWork, this));
00537   }
00538 
00539   void stopActiveAssociation()
00540   {
00541     ROS_INFO("stopActiveAssociation()");
00542     if (active_association_ == null_associate_goal_handle_)
00543       ROS_ERROR("stopActiveAssociation called with no active association.");
00544     
00545     wpa_supplicant_disassociate(wpa_s_, WLAN_REASON_DEAUTH_LEAVING);
00546   }
00547 
00548   void startActiveAssociation(AssociateActionServer::GoalHandle &gh)
00549   {
00550     ROS_INFO("startActiveAssociation()");
00551     if (active_association_ != null_associate_goal_handle_)
00552       ROS_ERROR("startActiveAssociation called with no active association.");
00553 
00554     wpa_bss *bss;
00555     boost::shared_ptr<const wpa_supplicant_node::AssociateGoal> goal = gh.getGoal();
00556     const u8 *bssid = &goal->bssid[0];
00557     std::string ssid = goal->ssid;
00558 
00559     bss = wpa_bss_get(wpa_s_, bssid, (u8 *) ssid.c_str(), ssid.length());
00560     
00561     wpa_ssid *net;
00562     for (net = wpa_s_->conf->ssid; net != NULL; net = net->next)
00563       if (ssid.length() == net->ssid_len && !os_memcmp(ssid.c_str(), net->ssid, net->ssid_len))
00564           break;
00565 
00566     if (bss && net)
00567     {
00568       gh.setAccepted();
00569       active_association_ = gh;
00570       wpa_supplicant_node::AssociateFeedback fbk;
00571       fbk.associated = false;
00572       active_association_.publishFeedback(fbk);
00573 
00574       eloop_register_timeout(5, 0, associateTimeoutHandler, wpa_s_, NULL);
00575       
00576       // Note, the following line may involve a call to assocFailed, so
00577       // everything needs to be in a consistent state before we call it.
00578       wpa_supplicant_associate(wpa_s_, bss, net);
00579     }
00580     else
00581     {
00582       if (!bss)
00583         ROS_ERROR("startActiveAssociation could not find requested bss.");
00584       if (!net)
00585         ROS_ERROR("startActiveAssociation could not find requested ssid.");
00586       gh.setRejected();
00587     }
00588     ROS_INFO("startActiveAssociation() done");
00589   }
00590 
00591   void associateWork()
00592   {
00593     boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00594     associate_work_requested_ = false;
00595 
00596     while (!associate_cancel_queue_.empty())
00597     {
00598       AssociateActionServer::GoalHandle gh = associate_cancel_queue_.front();
00599       associate_cancel_queue_.pop();
00600       if (gh == active_association_)
00601       {
00602         stopActiveAssociation();
00603         return; // We will get called when the disassociate completes.
00604       }
00605       else
00606         gh.setCanceled();
00607     }
00608 
00609     while (associate_goal_queue_.size() > 1)
00610     {
00611       AssociateActionServer::GoalHandle gh = associate_goal_queue_.front();
00612       associate_goal_queue_.pop();
00613       gh.setRejected();
00614     }
00615     
00616     if (!associate_goal_queue_.empty())
00617     {
00618       AssociateActionServer::GoalHandle gh = associate_goal_queue_.front();
00619       if (active_association_ != null_associate_goal_handle_ && active_association_.getGoal()->ssid != gh.getGoal()->ssid)
00620       {
00621         ROS_INFO("Unassociating prior to ESSID switch.");
00622         stopActiveAssociation();
00626         //active_association_.setAborted();
00627         //active_association_ = null_associate_goal_handle_; // Avoids error message in startActiveAssociation.
00628       }
00629       else
00630       {
00631         if (active_association_ != null_associate_goal_handle_)
00632         {
00633           active_association_.setAborted();
00634           active_association_ = null_associate_goal_handle_; // Avoids error message in startActiveAssociation.
00635         }
00636 
00637         associate_goal_queue_.pop();
00638       
00639         startActiveAssociation(gh);      
00640       }
00641     }
00642   }
00643 
00644   void associateGoalCallback(AssociateActionServer::GoalHandle &gh)
00645   {
00646     boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00647     
00648     associate_goal_queue_.push(gh);
00649     requestAssociateWork();
00650   }
00651 
00652   void associateCancelCallback(AssociateActionServer::GoalHandle &gh)
00653   {
00654     boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00655     
00656     associate_cancel_queue_.push(gh);
00657     requestAssociateWork();
00658   }
00659   
00660   void fillSecurityInformation(wpa_supplicant_node::SecurityProperties &props, struct wpa_ie_data &ie_data)
00661   {
00662       // key_mgmt
00663       if (ie_data.key_mgmt & WPA_KEY_MGMT_PSK)
00664           props.key_mgmt.push_back("wpa-psk");
00665       if (ie_data.key_mgmt & WPA_KEY_MGMT_FT_PSK)
00666           props.key_mgmt.push_back("wpa-ft-psk");
00667       if (ie_data.key_mgmt & WPA_KEY_MGMT_PSK_SHA256)
00668           props.key_mgmt.push_back("wpa-psk-sha256");
00669       if (ie_data.key_mgmt & WPA_KEY_MGMT_IEEE8021X)
00670           props.key_mgmt.push_back("wpa-eap");
00671       if (ie_data.key_mgmt & WPA_KEY_MGMT_FT_IEEE8021X)
00672           props.key_mgmt.push_back("wpa-ft-eap");
00673       if (ie_data.key_mgmt & WPA_KEY_MGMT_IEEE8021X_SHA256)
00674           props.key_mgmt.push_back("wpa-eap-sha256");
00675       if (ie_data.key_mgmt & WPA_KEY_MGMT_NONE)
00676           props.key_mgmt.push_back("wpa-none");
00677       
00678       // group
00679       switch (ie_data.group_cipher) {
00680       case WPA_CIPHER_WEP40:
00681           props.group.push_back("wep40");
00682           break;
00683       case WPA_CIPHER_TKIP:
00684           props.group.push_back("tkip");
00685           break;
00686       case WPA_CIPHER_CCMP:
00687           props.group.push_back("ccmp");
00688           break;
00689       case WPA_CIPHER_WEP104:
00690           props.group.push_back("wep104");
00691           break;
00692       }
00693       
00694       // pairwise
00695       if (ie_data.pairwise_cipher & WPA_CIPHER_TKIP)
00696           props.pairwise.push_back("tkip");
00697       if (ie_data.pairwise_cipher & WPA_CIPHER_CCMP)
00698           props.pairwise.push_back("ccmp");
00699   }
00700 
00701   void fillRosBss(wpa_supplicant_node::Bss &ros_bss, wpa_bss &bss)
00702   {
00703     ros_bss.stamp = ros::Time(bss.last_update.sec + 1e-6 * bss.last_update.usec);
00704     ros_bss.ssid.assign((const char *) bss.ssid, bss.ssid_len);
00705     memcpy(&ros_bss.bssid[0], bss.bssid, sizeof(bss.bssid));
00706     ros_bss.frequency = bss.freq;
00707     ros_bss.beacon_interval = bss.beacon_int;
00708     ros_bss.capabilities = bss.caps;
00709     ros_bss.quality = bss.qual;
00710     ros_bss.level = bss.level;
00711     ros_bss.noise = bss.noise;
00712 
00713     struct wpa_ie_data wpa_data;
00714     const u8 *ie;
00715     // wpa
00716     os_memset(&wpa_data, 0, sizeof(wpa_data));
00717     ie = wpa_bss_get_vendor_ie(&bss, WPA_IE_VENDOR_TYPE);
00718     if (ie)
00719         wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00720     fillSecurityInformation(ros_bss.wpa, wpa_data);
00721     // rsn
00722     os_memset(&wpa_data, 0, sizeof(wpa_data));
00723     ie = wpa_bss_get_ie(&bss, WLAN_EID_RSN);
00724     if (ie)
00725         wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00726     fillSecurityInformation(ros_bss.rsn, wpa_data);
00727   }
00728   
00729   void fillRosResp(wpa_supplicant_node::ScanResult &rslt, wpa_scan_results &scan_res)
00730   {
00731     rslt.success = true;
00732     rslt.bss.clear();
00733     for (size_t i = 0; i < scan_res.num; i++)
00734     {
00735       wpa_scan_res &cur = *scan_res.res[i];
00736       wpa_supplicant_node::Bss bss;
00737       const u8* ssid_ie = wpa_scan_get_ie(&cur, WLAN_EID_SSID);
00738       int ssid_len = ssid_ie ? ssid_ie[1] : 0;
00739       const char *ssid = ssid_ie ? (const char *) ssid_ie + 2 : "";
00740       bss.ssid.assign(ssid, ssid_len);
00741       memcpy(&bss.bssid[0], cur.bssid, sizeof(bss.bssid));
00742       bss.noise = cur.noise;
00743       bss.quality = cur.qual;
00744       bss.level = cur.level;
00745       bss.capabilities = cur.caps;
00746       bss.beacon_interval = cur.beacon_int;
00747       bss.frequency = cur.freq;
00748       bss.stamp = ros::Time::now() - ros::Duration(cur.age / 1000.0);
00749 
00750       struct wpa_ie_data wpa_data;
00751       const u8 *ie;
00752       // wpa
00753       os_memset(&wpa_data, 0, sizeof(wpa_data));
00754       ie = wpa_scan_get_vendor_ie(&cur, WPA_IE_VENDOR_TYPE);
00755       if (ie)
00756           wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00757       fillSecurityInformation(bss.wpa, wpa_data);
00758       // rsn
00759       os_memset(&wpa_data, 0, sizeof(wpa_data));
00760       ie = wpa_scan_get_ie(&cur, WLAN_EID_RSN);
00761       if (ie)
00762           wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00763       fillSecurityInformation(bss.rsn, wpa_data);
00764       rslt.bss.push_back(bss);
00765     }
00766   }
00767 
00768   void scanGoalCallback(ScanActionServer::GoalHandle &gh)
00769   {
00770     boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00771     
00772     ROS_INFO("scanGoalCallback()");
00773 
00774     scan_queue_.push(gh);
00775 
00776     if (current_scan_ == null_scan_goal_handle_)
00777       g_ros_api.addWork(boost::bind(&ros_interface::scanTryActivate, this));
00778   }
00779 
00780   void scanCancelCallback(ScanActionServer::GoalHandle &gh)
00781   {
00782     boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00783 
00784     unsigned int status = gh.getGoalStatus().status;
00785 
00786     if (current_scan_ == gh)
00787     {
00788       g_ros_api.addWork(boost::bind(&ros_interface::scanCancel, this, gh));
00789     }
00790     else
00791     {
00792       if (status != actionlib_msgs::GoalStatus::PREEMPTING)
00793         ROS_ERROR("scanCancelCallback called with unexpected goal status %i", status);
00794       gh.setCanceled();
00795     }      
00796   }
00797 
00798   void scanCancel(ScanActionServer::GoalHandle &gh)
00799   {
00800     ROS_INFO("scanCancel()");
00801     boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00802     
00803     // Are we still active (we may have succeeded before getting here)?
00804     if (current_scan_ == gh)
00805     {
00806       // FIXME Anything I can do here to actually cancel the scan?
00807       gh.setCanceled();
00808       current_scan_ = null_scan_goal_handle_;
00809       scanTryActivate();
00810     }
00811   }
00812 
00813   void lockedScanTryActivate()
00814   {
00815     // THIS VERSION SHOULD BE CALLED WITH THE LOCK HELD!
00816     ROS_INFO("scanTryActivate()");
00817     
00818     // Do we have a scan to activate?
00819     while (!scan_queue_.empty() && current_scan_ == null_scan_goal_handle_)
00820     {
00821       current_scan_ = scan_queue_.front();
00822       scan_queue_.pop();
00823 
00824       // Is this scan still pending? (Otherwise it has already been
00825       // canceled elsewhere.)
00826       if (current_scan_.getGoalStatus().status != actionlib_msgs::GoalStatus::PENDING)
00827       {
00828         ROS_INFO("Skipping canceled scan.");
00829         current_scan_ = null_scan_goal_handle_;
00830         continue;
00831       }
00832       
00833       boost::shared_ptr<const wpa_supplicant_node::ScanGoal> goal = current_scan_.getGoal();
00834       struct wpa_driver_scan_params wpa_req;
00835       bzero(&wpa_req, sizeof(wpa_req));
00836       
00837       std::string err = fillWpaReq(goal, wpa_req);
00838 
00839       if (err.empty())
00840       {
00841         current_scan_.setAccepted();
00842                                 
00843         ROS_INFO("Starting scan.");
00844         // The following timeout should never get used, as there is a
00845         // nearly identical one in the scan code. It is nevertheless here
00846         // just in case.
00847         int timeout;
00848         if (wpa_req.freqs == 0)
00849           timeout = 10000;
00850         else
00851           timeout = goal->frequencies.size() * 550;
00852         eloop_register_timeout(timeout / 1000, 1000 * (timeout % 1000), scanTimeoutHandler, wpa_s_, NULL);
00853         wpa_supplicant_trigger_scan(wpa_s_, &wpa_req);
00854       }
00855       else
00856       {
00857         current_scan_.setRejected(wpa_supplicant_node::ScanResult(), err);
00858         current_scan_ = null_scan_goal_handle_;
00859         continue;
00860       }
00861     }
00862       
00863     ROS_INFO("Leaving scanTryActivate");
00864     if (scan_queue_.empty())
00865       ROS_INFO("scan_queue_ is empty.");
00866     if (current_scan_ != null_scan_goal_handle_)
00867       ROS_INFO("A scan is active.");
00868   }
00869 
00870   static void scanTimeoutHandler(void *wpa_s, void *unused)
00871   {
00872     ROS_INFO("Scan timeout!");
00873     ((wpa_supplicant *) wpa_s)->ros_api->scanCompleted(NULL);
00874   }
00875  
00876   static void associateTimeoutHandler(void *wpa_s, void *unused)
00877   {
00878     ROS_INFO("Associate timeout!");
00879     static const u8 zeroMAC[] = "\0\0\0\0\0\0";
00880     ((wpa_supplicant *) wpa_s)->ros_api->assocFailed(zeroMAC, "Associatiot timed out.");
00881   }
00882  
00883   std::string fillWpaReq(boost::shared_ptr<const wpa_supplicant_node::ScanGoal> &g, struct wpa_driver_scan_params &wpa_req)
00884   {
00885     wpa_req.num_ssids = g->ssids.size();
00886 
00887 #define QUOTEME(x) #x
00888     if (wpa_req.num_ssids > WPAS_MAX_SCAN_SSIDS)
00889       return "Too many ESSIDs in scan request. Maximum number is "QUOTEME(WPAS_MAX_SCAN_SSIDS)".";
00890 
00891     for (unsigned int i = 0; i < wpa_req.num_ssids; i++)
00892     {
00893       wpa_req.ssids[i].ssid = (const u8 *) g->ssids[i].c_str();
00894       wpa_req.ssids[i].ssid_len = g->ssids[i].size();
00895 
00896       if (wpa_req.ssids[i].ssid_len > WPA_MAX_SSID_LEN)
00897         return "Ssid is too long. Maximum length is "QUOTEME(WPA_MAX_SSID_LEN)" characters.";
00898     }
00899 #undef QUOTEME
00900                                     
00901     current_scan_frequencies_.clear();
00902     int num_frequencies = g->frequencies.size();
00903     
00904     if (num_frequencies > 0)
00905     {
00906       for (int i = 0; i < num_frequencies; i++)
00907         current_scan_frequencies_.push_back(g->frequencies[i]);
00908       current_scan_frequencies_.push_back(0);
00909       wpa_req.freqs = &current_scan_frequencies_[0]; // Use wpa_req before the goal dies!
00910     }
00911   
00912     return "";
00913   }
00914 
00915   void scanTryActivate()
00916   {
00917     boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00918     lockedScanTryActivate();
00919   }
00920 };
00921 
00922 struct wpa_global *ros_interface::global = NULL;
00923 char ros_interface::country_code[2] = {0, 0};
00924 
00925 void RosApi::reconfigure(wpa_supplicant_node::WpaSupplicantNodeConfig &config, unsigned int& level)
00926 {
00927   boost::mutex::scoped_lock lock(mutex_);
00928   
00929   if (!first_call_to_reconfigure)
00930     {
00931       if (!waitForMainThread(lock))
00932         return; 
00933     }
00934 
00935   ros_interface::setCountryCode(config.country_code.c_str());
00936 }
00937 
00938 extern "C" {
00939 
00940 int ros_init(int *argc, char ***argv)
00941 {
00942   return g_ros_api.init(argc, argv);
00943 }
00944 
00945 void ros_init2()
00946 {
00947   return g_ros_api.init2();
00948 }
00949 
00950 void ros_deinit()
00951 {
00952   g_ros_api.uninit();
00953 }
00954 
00955 void ros_add_iface(wpa_supplicant *wpa_s)
00956 {
00957   wpa_s->ros_api = new ros_interface(ros::NodeHandle("wifi"), wpa_s);
00958 }
00959 
00960 void ros_remove_iface(wpa_supplicant *wpa_s)
00961 {
00962   delete wpa_s->ros_api;
00963   wpa_s->ros_api = NULL;
00964 }
00965 
00966 void ros_iface_idle(wpa_supplicant *wpa_s)
00967 {
00968   if (wpa_s->ros_api)
00969     wpa_s->ros_api->ifaceIdle();
00970 }
00971   
00972 void ros_scan_completed(wpa_supplicant *wpa_s, wpa_scan_results *scan_res)
00973 {
00974   if (wpa_s->ros_api)
00975     wpa_s->ros_api->scanCompleted(scan_res);
00976 }
00977 
00978 void ros_do_work(int, void *, void *)
00979 {
00980   g_ros_api.doWork();
00981 }
00982 
00983 void ros_assoc_failed(wpa_supplicant *wpa_s, const u8 *bssid, const char *reason)
00984 {
00985   if (wpa_s->ros_api)
00986     wpa_s->ros_api->assocFailed(bssid, reason);
00987 }
00988 
00989 void ros_assoc_success(wpa_supplicant *wpa_s)
00990 {
00991   if (wpa_s->ros_api)
00992     wpa_s->ros_api->assocSucceeded();
00993 }
00994 
00995 void ros_network_list_updated(wpa_supplicant *wpa_s)
00996 {
00997   if (wpa_s->ros_api)
00998     wpa_s->ros_api->publishNetworkList();
00999 }
01000 } // extern "C"


wpa_supplicant_node
Author(s): Package maintained by Blaise Gassend
autogenerated on Thu Jan 2 2014 11:25:15