hdlc.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397
  1. /* $Id$ */
  2. /*
  3. * Copyright (c) 2001-2010 Aaron Turner <aturner at synfin dot net>
  4. * Copyright (c) 2013-2024 Fred Klassen <tcpreplay at appneta dot com> - AppNeta
  5. *
  6. * The Tcpreplay Suite of tools is free software: you can redistribute it
  7. * and/or modify it under the terms of the GNU General Public License as
  8. * published by the Free Software Foundation, either version 3 of the
  9. * License, or with the authors permission any later version.
  10. *
  11. * The Tcpreplay Suite is distributed in the hope that it will be useful,
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14. * GNU General Public License for more details.
  15. *
  16. * You should have received a copy of the GNU General Public License
  17. * along with the Tcpreplay Suite. If not, see <http://www.gnu.org/licenses/>.
  18. */
  19. #include "hdlc.h"
  20. #include "dlt_utils.h"
  21. #include "tcpedit.h"
  22. #include "tcpedit_stub.h"
  23. #include <stdlib.h>
  24. #include <string.h>
  25. static char dlt_name[] = "hdlc";
  26. static char _U_ dlt_prefix[] = "hdlc";
  27. static uint16_t dlt_value = DLT_C_HDLC;
  28. /*
  29. * Function to register ourselves. This function is always called, regardless
  30. * of what DLT types are being used, so it shouldn't be allocating extra buffers
  31. * or anything like that (use the dlt_hdlc_init() function below for that).
  32. * Tasks:
  33. * - Create a new plugin struct
  34. * - Fill out the provides/requires bit masks. Note: Only specify which fields are
  35. * actually in the header.
  36. * - Add the plugin to the context's plugin chain
  37. * Returns: TCPEDIT_ERROR | TCPEDIT_OK | TCPEDIT_WARN
  38. */
  39. int
  40. dlt_hdlc_register(tcpeditdlt_t *ctx)
  41. {
  42. tcpeditdlt_plugin_t *plugin;
  43. assert(ctx);
  44. /* create a new plugin structure */
  45. plugin = tcpedit_dlt_newplugin();
  46. /* FIXME: set what we provide & require */
  47. plugin->provides += PLUGIN_MASK_PROTO;
  48. plugin->
  49. requires
  50. += PLUGIN_MASK_PROTO;
  51. /* what is our DLT value? */
  52. plugin->dlt = dlt_value;
  53. /* set the prefix name of our plugin. This is also used as the prefix for our options */
  54. plugin->name = safe_strdup(dlt_prefix);
  55. /*
  56. * Point to our functions, note, you need a function for EVERY method.
  57. * Even if it is only an empty stub returning success.
  58. */
  59. plugin->plugin_init = dlt_hdlc_init;
  60. plugin->plugin_cleanup = dlt_hdlc_cleanup;
  61. plugin->plugin_parse_opts = dlt_hdlc_parse_opts;
  62. plugin->plugin_decode = dlt_hdlc_decode;
  63. plugin->plugin_encode = dlt_hdlc_encode;
  64. plugin->plugin_proto = dlt_hdlc_proto;
  65. plugin->plugin_l2addr_type = dlt_hdlc_l2addr_type;
  66. plugin->plugin_l2len = dlt_hdlc_l2len;
  67. plugin->plugin_get_layer3 = dlt_hdlc_get_layer3;
  68. plugin->plugin_merge_layer3 = dlt_hdlc_merge_layer3;
  69. plugin->plugin_get_mac = dlt_hdlc_get_mac;
  70. /* add it to the available plugin list */
  71. return tcpedit_dlt_addplugin(ctx, plugin);
  72. }
  73. /*
  74. * Initializer function. This function is called only once, if and only if
  75. * this plugin will be utilized. Remember, if you need to keep track of any state,
  76. * store it in your plugin->config, not a global!
  77. * Returns: TCPEDIT_ERROR | TCPEDIT_OK | TCPEDIT_WARN
  78. */
  79. int
  80. dlt_hdlc_init(tcpeditdlt_t *ctx)
  81. {
  82. tcpeditdlt_plugin_t *plugin;
  83. hdlc_config_t *config;
  84. assert(ctx);
  85. if ((plugin = tcpedit_dlt_getplugin(ctx, dlt_value)) == NULL) {
  86. tcpedit_seterr(ctx->tcpedit, "Unable to initialize unregistered plugin %s", dlt_name);
  87. return TCPEDIT_ERROR;
  88. }
  89. /* allocate memory for our deocde extra data */
  90. if (ctx->decoded_extra_size > 0) {
  91. if (ctx->decoded_extra_size < sizeof(hdlc_extra_t)) {
  92. ctx->decoded_extra_size = sizeof(hdlc_extra_t);
  93. ctx->decoded_extra = safe_realloc(ctx->decoded_extra, ctx->decoded_extra_size);
  94. }
  95. } else {
  96. ctx->decoded_extra_size = sizeof(hdlc_extra_t);
  97. ctx->decoded_extra = safe_malloc(ctx->decoded_extra_size);
  98. }
  99. /* allocate memory for our config data */
  100. plugin->config_size = sizeof(hdlc_config_t);
  101. plugin->config = safe_malloc(plugin->config_size);
  102. config = (hdlc_config_t *)plugin->config;
  103. /* default to unset */
  104. config->address = 65535;
  105. config->control = 65535;
  106. return TCPEDIT_OK; /* success */
  107. }
  108. /*
  109. * Since this is used in a library, we should manually clean up after ourselves
  110. * Unless you allocated some memory in dlt_hdlc_init(), this is just an stub.
  111. * Returns: TCPEDIT_ERROR | TCPEDIT_OK | TCPEDIT_WARN
  112. */
  113. int
  114. dlt_hdlc_cleanup(tcpeditdlt_t *ctx)
  115. {
  116. tcpeditdlt_plugin_t *plugin;
  117. assert(ctx);
  118. if ((plugin = tcpedit_dlt_getplugin(ctx, dlt_value)) == NULL) {
  119. tcpedit_seterr(ctx->tcpedit, "Unable to cleanup unregistered plugin %s", dlt_name);
  120. return TCPEDIT_ERROR;
  121. }
  122. safe_free(plugin->name);
  123. plugin->name = NULL;
  124. safe_free(plugin->config);
  125. plugin->config = NULL;
  126. plugin->config_size = 0;
  127. return TCPEDIT_OK; /* success */
  128. }
  129. /*
  130. * This is where you should define all your AutoGen AutoOpts option parsing.
  131. * Any user specified option should have it's bit turned on in the 'provides'
  132. * bit mask.
  133. * Returns: TCPEDIT_ERROR | TCPEDIT_OK | TCPEDIT_WARN
  134. */
  135. int
  136. dlt_hdlc_parse_opts(tcpeditdlt_t *ctx)
  137. {
  138. tcpeditdlt_plugin_t *plugin;
  139. hdlc_config_t *config;
  140. assert(ctx);
  141. plugin = tcpedit_dlt_getplugin(ctx, dlt_value);
  142. if (!plugin)
  143. return TCPEDIT_ERROR;
  144. config = plugin->config;
  145. if (plugin->config_size < sizeof(*config))
  146. return TCPEDIT_ERROR;
  147. if (HAVE_OPT(HDLC_CONTROL)) {
  148. config->control = (uint16_t)OPT_VALUE_HDLC_CONTROL;
  149. }
  150. if (HAVE_OPT(HDLC_ADDRESS)) {
  151. config->address = (uint16_t)OPT_VALUE_HDLC_ADDRESS;
  152. }
  153. return TCPEDIT_OK; /* success */
  154. }
  155. /*
  156. * Function to decode the layer 2 header in the packet.
  157. * You need to fill out:
  158. * - ctx->l2len
  159. * - ctx->srcaddr
  160. * - ctx->dstaddr
  161. * - ctx->proto
  162. * - ctx->decoded_extra
  163. * Returns: TCPEDIT_ERROR | TCPEDIT_OK | TCPEDIT_WARN
  164. */
  165. int
  166. dlt_hdlc_decode(tcpeditdlt_t *ctx, const u_char *packet, int pktlen)
  167. {
  168. cisco_hdlc_t *hdlc;
  169. hdlc_extra_t *extra;
  170. assert(ctx);
  171. assert(packet);
  172. if ((size_t)pktlen < sizeof(*hdlc))
  173. return TCPEDIT_ERROR;
  174. if (ctx->decoded_extra_size < sizeof(*extra))
  175. return TCPEDIT_ERROR;
  176. extra = (hdlc_extra_t *)ctx->decoded_extra;
  177. hdlc = (cisco_hdlc_t *)packet;
  178. ctx->proto = hdlc->protocol;
  179. ctx->l2len = 4;
  180. extra->address = hdlc->address;
  181. extra->control = hdlc->control;
  182. return TCPEDIT_OK; /* success */
  183. }
  184. /*
  185. * Function to encode the layer 2 header back into the packet.
  186. * Returns: total packet len or TCPEDIT_ERROR
  187. */
  188. int
  189. dlt_hdlc_encode(tcpeditdlt_t *ctx, u_char *packet, int pktlen, _U_ tcpr_dir_t dir)
  190. {
  191. cisco_hdlc_t *hdlc;
  192. hdlc_config_t *config = NULL;
  193. hdlc_extra_t *extra = NULL;
  194. tcpeditdlt_plugin_t *plugin = NULL;
  195. int newpktlen;
  196. assert(ctx);
  197. assert(packet);
  198. if ((size_t)pktlen < sizeof(*hdlc))
  199. return TCPEDIT_ERROR;
  200. if (ctx->decoded_extra_size < sizeof(*extra))
  201. return TCPEDIT_ERROR;
  202. /* Make room for our new l2 header if old l2len != 4 */
  203. if (ctx->l2len > 4) {
  204. memmove(packet + 4, packet + ctx->l2len, pktlen - ctx->l2len);
  205. } else if (ctx->l2len < 4) {
  206. u_char *tmpbuff = safe_malloc(pktlen);
  207. memcpy(tmpbuff, packet, pktlen);
  208. memcpy(packet + 4, (tmpbuff + ctx->l2len), pktlen - ctx->l2len);
  209. safe_free(tmpbuff);
  210. }
  211. /* update the total packet length */
  212. newpktlen = pktlen + 4 - ctx->l2len;
  213. /*
  214. * HDLC doesn't support direction, since we have no real src/dst addresses
  215. * to deal with, so we just use the original packet data or option data
  216. */
  217. hdlc = (cisco_hdlc_t *)packet;
  218. plugin = tcpedit_dlt_getplugin(ctx, dlt_value);
  219. if (!plugin)
  220. return TCPEDIT_ERROR;
  221. config = plugin->config;
  222. if (plugin->config_size < sizeof(*config))
  223. return TCPEDIT_ERROR;
  224. extra = (hdlc_extra_t *)ctx->decoded_extra;
  225. if (ctx->decoded_extra_size < sizeof(*extra))
  226. return TCPEDIT_ERROR;
  227. /* set the address field */
  228. if (config->address < 65535) {
  229. hdlc->address = (uint8_t)config->address;
  230. } else if (extra->hdlc) {
  231. hdlc->address = extra->hdlc;
  232. } else {
  233. tcpedit_seterr(ctx->tcpedit, "%s", "Non-HDLC packet requires --hdlc-address");
  234. return TCPEDIT_ERROR;
  235. }
  236. /* set the control field */
  237. if (config->control < 65535) {
  238. hdlc->control = (uint8_t)config->control;
  239. } else if (extra->hdlc) {
  240. hdlc->control = extra->hdlc;
  241. } else {
  242. tcpedit_seterr(ctx->tcpedit, "%s", "Non-HDLC packet requires --hdlc-control");
  243. return TCPEDIT_ERROR;
  244. }
  245. /* copy over our protocol */
  246. hdlc->protocol = ctx->proto;
  247. return newpktlen; /* success */
  248. }
  249. /*
  250. * Function returns the Layer 3 protocol type of the given packet, or TCPEDIT_ERROR on error
  251. */
  252. int
  253. dlt_hdlc_proto(tcpeditdlt_t *ctx, const u_char *packet, int pktlen)
  254. {
  255. cisco_hdlc_t *hdlc;
  256. assert(ctx);
  257. assert(packet);
  258. if (pktlen < 4)
  259. return TCPEDIT_ERROR;
  260. hdlc = (cisco_hdlc_t *)packet;
  261. return hdlc->protocol;
  262. }
  263. /*
  264. * Function returns a pointer to the layer 3 protocol header or NULL on error
  265. */
  266. u_char *
  267. dlt_hdlc_get_layer3(tcpeditdlt_t *ctx, u_char *packet, int pktlen)
  268. {
  269. int l2len;
  270. assert(ctx);
  271. assert(packet);
  272. l2len = dlt_hdlc_l2len(ctx, packet, pktlen);
  273. if (l2len == -1 || pktlen < l2len)
  274. return NULL;
  275. return tcpedit_dlt_l3data_copy(ctx, packet, pktlen, l2len);
  276. }
  277. /*
  278. * function merges the packet (containing L2 and old L3) with the l3data buffer
  279. * containing the new l3 data. Note, if L2 % 4 == 0, then they're pointing to the
  280. * same buffer, otherwise there was a memcpy involved on strictly aligned architectures
  281. * like SPARC
  282. */
  283. u_char *
  284. dlt_hdlc_merge_layer3(tcpeditdlt_t *ctx, u_char *packet, int pktlen, u_char *ipv4_data, u_char *ipv6_data)
  285. {
  286. int l2len;
  287. assert(ctx);
  288. assert(packet);
  289. assert(ipv4_data || ipv6_data);
  290. l2len = dlt_hdlc_l2len(ctx, packet, pktlen);
  291. if (l2len == -1 || pktlen < l2len)
  292. return NULL;
  293. return tcpedit_dlt_l3data_merge(ctx, packet, pktlen, ipv4_data ?: ipv6_data, l2len);
  294. }
  295. /*
  296. * return the length of the L2 header of the current packet
  297. */
  298. int
  299. dlt_hdlc_l2len(tcpeditdlt_t *ctx, const u_char *packet, int pktlen)
  300. {
  301. assert(ctx);
  302. assert(packet);
  303. if (pktlen < 4)
  304. return -1;
  305. /* HDLC is a static 4 bytes */
  306. return 4;
  307. }
  308. /*
  309. * return a static pointer to the source/destination MAC address
  310. * return NULL on error/address doesn't exist
  311. */
  312. u_char *
  313. dlt_hdlc_get_mac(tcpeditdlt_t *ctx, tcpeditdlt_mac_type_t mac, const u_char *packet, int pktlen)
  314. {
  315. assert(ctx);
  316. assert(packet);
  317. if (pktlen < 14)
  318. return NULL;
  319. /* FIXME: return a ptr to the source or dest mac address. */
  320. switch (mac) {
  321. case SRC_MAC:
  322. return (NULL);
  323. case DST_MAC:
  324. memcpy(ctx->dstmac, packet, 2);
  325. return (ctx->dstmac);
  326. default:
  327. errx(-1, "Invalid tcpeditdlt_mac_type_t: %d", mac);
  328. }
  329. }
  330. tcpeditdlt_l2addr_type_t
  331. dlt_hdlc_l2addr_type(void)
  332. {
  333. return C_HDLC;
  334. }