main.c 9.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382
  1. #include <unistd.h>
  2. #include <stdio.h>
  3. #include <stdbool.h>
  4. #include <errno.h>
  5. #include <netinet/in.h>
  6. #include <linux/if.h>
  7. #include <linux/if_arp.h>
  8. #include <linux/can.h>
  9. #include <linux/can/raw.h>
  10. #include <linux/can/netlink.h>
  11. #include <netlink/version.h>
  12. #include <netlink/route/link.h>
  13. #include <netlink/route/link/can.h>
  14. struct opts {
  15. char *ifname;
  16. bool do_bring_up;
  17. bool do_bring_down;
  18. bool do_set_bitrate;
  19. unsigned bitrate;
  20. bool do_set_sample_point;
  21. unsigned sample_point;
  22. bool do_set_fd_enable;
  23. bool fd_enable;
  24. bool do_set_fd_bitrate;
  25. unsigned fd_bitrate;
  26. bool do_set_fd_sample_point;
  27. unsigned fd_sample_point;
  28. bool do_set_restart_ms;
  29. unsigned restart_ms;
  30. bool do_set_mode_listen_only;
  31. bool mode_listen_only;
  32. bool do_set_mode_oneshot;
  33. bool mode_oneshot;
  34. bool do_set_mode_triple_sampling;
  35. bool mode_triple_sampling;
  36. };
  37. struct nldata {
  38. struct nl_sock *sock;
  39. struct nl_cache *cache;
  40. struct rtnl_link *link;
  41. };
  42. void print_usage(char *program_name)
  43. {
  44. fprintf(
  45. stderr,
  46. "Usage: %s -i <socketcan interface>\n"
  47. " -d bring interface down before configuration\n"
  48. " -u bring interface up after configuration\n"
  49. " -b <n> set bitrate to n bits per second\n"
  50. " -p <n> set sample point to n/1000, e.g. 875 for 87.5%%\n"
  51. " -f <on|off> enable/disable CanFD\n"
  52. " -B <n> set fd bitrate to n bits per second\n"
  53. " -P <n> set fd sample point to n/1000, e.g. 875 for 87.5%%\n"
  54. " -r <n> set auto restart time to n milliseconds, 0 for disable\n"
  55. " -l <on|off> enable/disable listen-only mode\n"
  56. " -o <on|off> enable/disable one-shot mode\n"
  57. " -3 <on|off> enable/disable triple-sampling mode\n"
  58. "\n",
  59. program_name
  60. );
  61. }
  62. bool atobool(char *name, char *arg) {
  63. if (strcmp(arg, "on")==0) {
  64. return true;
  65. } else if (strcmp(arg, "off")==0) {
  66. return false;
  67. } else {
  68. fprintf(stderr, "Error: Argument of %s must be \"on\" or \"off\", not \"%s\"\n", name, arg);
  69. exit(EXIT_FAILURE);
  70. }
  71. }
  72. int parse_opts(int argc, char *argv[], struct opts *opts)
  73. {
  74. int opt;
  75. memset(opts, 0, sizeof(*opts));
  76. while ((opt = getopt(argc, argv, "i:udb:p:f:B:P:r:l:o:3:h")) != -1) {
  77. switch (opt) {
  78. case 'i':
  79. opts->ifname = optarg;
  80. break;
  81. case 'u':
  82. opts->do_bring_up = true;
  83. break;
  84. case 'd':
  85. opts->do_bring_down = true;
  86. break;
  87. case 'b':
  88. opts->do_set_bitrate = true;
  89. opts->bitrate = atoi(optarg);
  90. break;
  91. case 'p':
  92. opts->do_set_sample_point = true;
  93. opts->sample_point = atoi(optarg);
  94. break;
  95. case 'f':
  96. opts->do_set_fd_enable = true;
  97. opts->fd_enable = atobool("-f", optarg);
  98. break;
  99. case 'B':
  100. opts->do_set_fd_bitrate = true;
  101. opts->fd_bitrate = atoi(optarg);
  102. break;
  103. case 'P':
  104. opts->do_set_fd_sample_point = true;
  105. opts->fd_sample_point = atoi(optarg);
  106. break;
  107. case 'r':
  108. opts->do_set_restart_ms = true;
  109. opts->restart_ms = atoi(optarg);
  110. break;
  111. case 'l':
  112. opts->do_set_mode_listen_only = true;
  113. opts->mode_listen_only = atobool("-l", optarg);
  114. break;
  115. case 'o':
  116. opts->do_set_mode_oneshot = true;
  117. opts->mode_oneshot = atobool("-o", optarg);
  118. break;
  119. case '3':
  120. opts->do_set_mode_triple_sampling = true;
  121. opts->mode_triple_sampling = atobool("-3", optarg);
  122. break;
  123. case 'h':
  124. default:
  125. print_usage(argv[0]);
  126. return EXIT_FAILURE;
  127. }
  128. }
  129. if (opts->ifname == 0) {
  130. print_usage(argv[0]);
  131. return EXIT_FAILURE;
  132. }
  133. return EXIT_SUCCESS;
  134. }
  135. int netlink_connect(struct nldata *nldata, char *ifname)
  136. {
  137. memset(nldata, 0, sizeof(*nldata));
  138. nldata->sock = nl_socket_alloc();
  139. if ( nldata->sock == NULL ) {
  140. perror("cannot allocate netlink socket");
  141. return EXIT_FAILURE;
  142. }
  143. if ( nl_connect(nldata->sock, NETLINK_ROUTE) != 0 ) {
  144. perror("cannot connect to kernel netlink interface");
  145. } else {
  146. if ( rtnl_link_alloc_cache(nldata->sock, AF_UNSPEC, &nldata->cache) != 0 ) {
  147. perror("cannot allocate link cache");
  148. } else {
  149. nldata->link = rtnl_link_get_by_name(nldata->cache, ifname);
  150. if (nldata->link == 0) {
  151. fprintf(stderr, "cannot find interface %s\n", ifname);
  152. } else {
  153. if (rtnl_link_is_can(nldata->link)) {
  154. return EXIT_SUCCESS;
  155. } else {
  156. fprintf(stderr, "not a can interface: %s\n", ifname);
  157. }
  158. rtnl_link_put(nldata->link);
  159. nldata->link = NULL;
  160. }
  161. nl_cache_free(nldata->cache);
  162. nldata->cache = NULL;
  163. }
  164. nl_close(nldata->sock);
  165. }
  166. nl_socket_free(nldata->sock);
  167. nldata->sock = NULL;
  168. return EXIT_FAILURE;
  169. }
  170. int netlink_close(struct nldata *nldata)
  171. {
  172. if (nldata->link) {
  173. rtnl_link_put(nldata->link);
  174. nldata->link = NULL;
  175. }
  176. if (nldata->cache) {
  177. nl_cache_free(nldata->cache);
  178. nldata->cache = NULL;
  179. }
  180. if (nldata->sock) {
  181. nl_close(nldata->sock);
  182. nl_socket_free(nldata->sock);
  183. nldata->sock = NULL;
  184. }
  185. return EXIT_SUCCESS;
  186. }
  187. bool netlink_new_can_request(struct rtnl_link **req)
  188. {
  189. if (*req != NULL) {
  190. rtnl_link_put(*req);
  191. }
  192. *req = rtnl_link_alloc();
  193. if (*req != NULL) {
  194. if (rtnl_link_set_type(*req, "can") != 0) {
  195. rtnl_link_put(*req);
  196. *req = NULL;
  197. }
  198. }
  199. if (*req == NULL) {
  200. fprintf(stderr, "cannot create netlink request object\n");
  201. }
  202. return *req != NULL;
  203. }
  204. int main(int argc, char *argv[])
  205. {
  206. struct opts opts;
  207. struct nldata nldata;
  208. struct rtnl_link *req = NULL;
  209. int retval = EXIT_FAILURE;
  210. if (parse_opts(argc, argv, &opts) != 0) {
  211. exit(EXIT_FAILURE);
  212. }
  213. if (netlink_connect(&nldata, opts.ifname) != 0) {
  214. exit(EXIT_FAILURE);
  215. }
  216. do {
  217. if (opts.do_bring_down) {
  218. if (!netlink_new_can_request(&req)) {
  219. break;
  220. }
  221. rtnl_link_unset_flags(req, IFF_UP);
  222. if (rtnl_link_change(nldata.sock, nldata.link, req, 0) != 0) {
  223. perror("cannot bring interface down");
  224. break;
  225. }
  226. }
  227. if (!netlink_new_can_request(&req)) {
  228. exit(EXIT_FAILURE);
  229. }
  230. if (opts.do_set_bitrate) {
  231. if (rtnl_link_can_set_bitrate(req, opts.bitrate) != 0) {
  232. fprintf(stderr, "cannot set bitrate to %d\n", opts.bitrate);
  233. break;
  234. }
  235. }
  236. if (opts.do_set_sample_point) {
  237. if (rtnl_link_can_set_sample_point(req, opts.sample_point) != 0) {
  238. fprintf(stderr, "cannot set sample point to %.3f\n", opts.sample_point/1000.0);
  239. break;
  240. }
  241. }
  242. if (opts.do_set_fd_enable || opts.do_set_fd_bitrate || opts.do_set_fd_sample_point) {
  243. fprintf(stderr, "warning: canfd functions not implemented yet.\n");
  244. }
  245. if (opts.do_set_restart_ms) {
  246. if (rtnl_link_can_set_restart_ms(req, opts.restart_ms) != 0) {
  247. fprintf(stderr, "cannot set restart_ms to %d\n", opts.restart_ms);
  248. }
  249. }
  250. if (opts.do_set_mode_listen_only) {
  251. if (opts.mode_listen_only) {
  252. printf("setting listen-only flag to true\n");
  253. rtnl_link_can_set_ctrlmode(nldata.link, CAN_CTRLMODE_LISTENONLY);
  254. } else {
  255. printf("setting listen-only flag to false\n");
  256. rtnl_link_can_unset_ctrlmode(nldata.link, CAN_CTRLMODE_LISTENONLY);
  257. }
  258. }
  259. if (opts.do_set_mode_triple_sampling) {
  260. if (opts.mode_triple_sampling) {
  261. rtnl_link_can_set_ctrlmode(req, CAN_CTRLMODE_3_SAMPLES);
  262. } else {
  263. rtnl_link_can_unset_ctrlmode(req, CAN_CTRLMODE_3_SAMPLES);
  264. }
  265. }
  266. if (opts.do_set_mode_oneshot) {
  267. if (opts.mode_oneshot) {
  268. rtnl_link_can_set_ctrlmode(req, CAN_CTRLMODE_ONE_SHOT);
  269. } else {
  270. rtnl_link_can_unset_ctrlmode(req, CAN_CTRLMODE_ONE_SHOT);
  271. }
  272. }
  273. if (rtnl_link_change(nldata.sock, nldata.link, req, 0) != 0) {
  274. perror("cannot set link configuration");
  275. break;
  276. }
  277. if (opts.do_bring_up) {
  278. if (!netlink_new_can_request(&req)) {
  279. break;
  280. }
  281. rtnl_link_set_flags(req, IFF_UP);
  282. if (rtnl_link_change(nldata.sock, nldata.link, req, 0) != 0) {
  283. perror("cannot bring interface up");
  284. break;
  285. }
  286. }
  287. retval = EXIT_SUCCESS;
  288. } while (false);
  289. if (req != NULL) {
  290. rtnl_link_put(req);
  291. }
  292. netlink_close(&nldata);
  293. exit(retval);
  294. }