cdp.c 7.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360
  1. // SPDX-License-Identifier: GPL-2.0
  2. /*
  3. * Copied from Linux Monitor (LiMon) - Networking.
  4. *
  5. * Copyright 1994 - 2000 Neil Russell.
  6. * (See License)
  7. * Copyright 2000 Roland Borde
  8. * Copyright 2000 Paolo Scaffardi
  9. * Copyright 2000-2002 Wolfgang Denk, wd@denx.de
  10. */
  11. #include <common.h>
  12. #include <net.h>
  13. #include "cdp.h"
  14. /* Ethernet bcast address */
  15. const u8 net_cdp_ethaddr[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
  16. #define CDP_DEVICE_ID_TLV 0x0001
  17. #define CDP_ADDRESS_TLV 0x0002
  18. #define CDP_PORT_ID_TLV 0x0003
  19. #define CDP_CAPABILITIES_TLV 0x0004
  20. #define CDP_VERSION_TLV 0x0005
  21. #define CDP_PLATFORM_TLV 0x0006
  22. #define CDP_NATIVE_VLAN_TLV 0x000a
  23. #define CDP_APPLIANCE_VLAN_TLV 0x000e
  24. #define CDP_TRIGGER_TLV 0x000f
  25. #define CDP_POWER_CONSUMPTION_TLV 0x0010
  26. #define CDP_SYSNAME_TLV 0x0014
  27. #define CDP_SYSOBJECT_TLV 0x0015
  28. #define CDP_MANAGEMENT_ADDRESS_TLV 0x0016
  29. #define CDP_TIMEOUT 250UL /* one packet every 250ms */
  30. static int cdp_seq;
  31. static int cdp_ok;
  32. ushort cdp_native_vlan;
  33. ushort cdp_appliance_vlan;
  34. static const uchar cdp_snap_hdr[8] = {
  35. 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x0C, 0x20, 0x00 };
  36. static ushort cdp_compute_csum(const uchar *buff, ushort len)
  37. {
  38. ushort csum;
  39. int odd;
  40. ulong result = 0;
  41. ushort leftover;
  42. ushort *p;
  43. if (len > 0) {
  44. odd = 1 & (ulong)buff;
  45. if (odd) {
  46. result = *buff << 8;
  47. len--;
  48. buff++;
  49. }
  50. while (len > 1) {
  51. p = (ushort *)buff;
  52. result += *p++;
  53. buff = (uchar *)p;
  54. if (result & 0x80000000)
  55. result = (result & 0xFFFF) + (result >> 16);
  56. len -= 2;
  57. }
  58. if (len) {
  59. leftover = (signed short)(*(const signed char *)buff);
  60. /*
  61. * CISCO SUCKS big time! (and blows too):
  62. * CDP uses the IP checksum algorithm with a twist;
  63. * for the last byte it *sign* extends and sums.
  64. */
  65. result = (result & 0xffff0000) |
  66. ((result + leftover) & 0x0000ffff);
  67. }
  68. while (result >> 16)
  69. result = (result & 0xFFFF) + (result >> 16);
  70. if (odd)
  71. result = ((result >> 8) & 0xff) |
  72. ((result & 0xff) << 8);
  73. }
  74. /* add up 16-bit and 17-bit words for 17+c bits */
  75. result = (result & 0xffff) + (result >> 16);
  76. /* add up 16-bit and 2-bit for 16+c bit */
  77. result = (result & 0xffff) + (result >> 16);
  78. /* add up carry.. */
  79. result = (result & 0xffff) + (result >> 16);
  80. /* negate */
  81. csum = ~(ushort)result;
  82. /* run time endian detection */
  83. if (csum != htons(csum)) /* little endian */
  84. csum = htons(csum);
  85. return csum;
  86. }
  87. static int cdp_send_trigger(void)
  88. {
  89. uchar *pkt;
  90. ushort *s;
  91. ushort *cp;
  92. struct ethernet_hdr *et;
  93. int len;
  94. ushort chksum;
  95. #if defined(CONFIG_CDP_DEVICE_ID) || defined(CONFIG_CDP_PORT_ID) || \
  96. defined(CONFIG_CDP_VERSION) || defined(CONFIG_CDP_PLATFORM)
  97. char buf[32];
  98. #endif
  99. pkt = net_tx_packet;
  100. et = (struct ethernet_hdr *)pkt;
  101. /* NOTE: trigger sent not on any VLAN */
  102. /* form ethernet header */
  103. memcpy(et->et_dest, net_cdp_ethaddr, 6);
  104. memcpy(et->et_src, net_ethaddr, 6);
  105. pkt += ETHER_HDR_SIZE;
  106. /* SNAP header */
  107. memcpy((uchar *)pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr));
  108. pkt += sizeof(cdp_snap_hdr);
  109. /* CDP header */
  110. *pkt++ = 0x02; /* CDP version 2 */
  111. *pkt++ = 180; /* TTL */
  112. s = (ushort *)pkt;
  113. cp = s;
  114. /* checksum (0 for later calculation) */
  115. *s++ = htons(0);
  116. /* CDP fields */
  117. #ifdef CONFIG_CDP_DEVICE_ID
  118. *s++ = htons(CDP_DEVICE_ID_TLV);
  119. *s++ = htons(CONFIG_CDP_DEVICE_ID);
  120. sprintf(buf, CONFIG_CDP_DEVICE_ID_PREFIX "%pm", net_ethaddr);
  121. memcpy((uchar *)s, buf, 16);
  122. s += 16 / 2;
  123. #endif
  124. #ifdef CONFIG_CDP_PORT_ID
  125. *s++ = htons(CDP_PORT_ID_TLV);
  126. memset(buf, 0, sizeof(buf));
  127. sprintf(buf, CONFIG_CDP_PORT_ID, eth_get_dev_index());
  128. len = strlen(buf);
  129. if (len & 1) /* make it even */
  130. len++;
  131. *s++ = htons(len + 4);
  132. memcpy((uchar *)s, buf, len);
  133. s += len / 2;
  134. #endif
  135. #ifdef CONFIG_CDP_CAPABILITIES
  136. *s++ = htons(CDP_CAPABILITIES_TLV);
  137. *s++ = htons(8);
  138. *(ulong *)s = htonl(CONFIG_CDP_CAPABILITIES);
  139. s += 2;
  140. #endif
  141. #ifdef CONFIG_CDP_VERSION
  142. *s++ = htons(CDP_VERSION_TLV);
  143. memset(buf, 0, sizeof(buf));
  144. strcpy(buf, CONFIG_CDP_VERSION);
  145. len = strlen(buf);
  146. if (len & 1) /* make it even */
  147. len++;
  148. *s++ = htons(len + 4);
  149. memcpy((uchar *)s, buf, len);
  150. s += len / 2;
  151. #endif
  152. #ifdef CONFIG_CDP_PLATFORM
  153. *s++ = htons(CDP_PLATFORM_TLV);
  154. memset(buf, 0, sizeof(buf));
  155. strcpy(buf, CONFIG_CDP_PLATFORM);
  156. len = strlen(buf);
  157. if (len & 1) /* make it even */
  158. len++;
  159. *s++ = htons(len + 4);
  160. memcpy((uchar *)s, buf, len);
  161. s += len / 2;
  162. #endif
  163. #ifdef CONFIG_CDP_TRIGGER
  164. *s++ = htons(CDP_TRIGGER_TLV);
  165. *s++ = htons(8);
  166. *(ulong *)s = htonl(CONFIG_CDP_TRIGGER);
  167. s += 2;
  168. #endif
  169. #ifdef CONFIG_CDP_POWER_CONSUMPTION
  170. *s++ = htons(CDP_POWER_CONSUMPTION_TLV);
  171. *s++ = htons(6);
  172. *s++ = htons(CONFIG_CDP_POWER_CONSUMPTION);
  173. #endif
  174. /* length of ethernet packet */
  175. len = (uchar *)s - ((uchar *)net_tx_packet + ETHER_HDR_SIZE);
  176. et->et_protlen = htons(len);
  177. len = ETHER_HDR_SIZE + sizeof(cdp_snap_hdr);
  178. chksum = cdp_compute_csum((uchar *)net_tx_packet + len,
  179. (uchar *)s - (net_tx_packet + len));
  180. if (chksum == 0)
  181. chksum = 0xFFFF;
  182. *cp = htons(chksum);
  183. net_send_packet(net_tx_packet, (uchar *)s - net_tx_packet);
  184. return 0;
  185. }
  186. static void cdp_timeout_handler(void)
  187. {
  188. cdp_seq++;
  189. if (cdp_seq < 3) {
  190. net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
  191. cdp_send_trigger();
  192. return;
  193. }
  194. /* if not OK try again */
  195. if (!cdp_ok)
  196. net_start_again();
  197. else
  198. net_set_state(NETLOOP_SUCCESS);
  199. }
  200. void cdp_receive(const uchar *pkt, unsigned len)
  201. {
  202. const uchar *t;
  203. const ushort *ss;
  204. ushort type, tlen;
  205. ushort vlan, nvlan;
  206. /* minimum size? */
  207. if (len < sizeof(cdp_snap_hdr) + 4)
  208. goto pkt_short;
  209. /* check for valid CDP SNAP header */
  210. if (memcmp(pkt, cdp_snap_hdr, sizeof(cdp_snap_hdr)) != 0)
  211. return;
  212. pkt += sizeof(cdp_snap_hdr);
  213. len -= sizeof(cdp_snap_hdr);
  214. /* Version of CDP protocol must be >= 2 and TTL != 0 */
  215. if (pkt[0] < 0x02 || pkt[1] == 0)
  216. return;
  217. /*
  218. * if version is greater than 0x02 maybe we'll have a problem;
  219. * output a warning
  220. */
  221. if (pkt[0] != 0x02)
  222. printf("**WARNING: CDP packet received with a protocol version "
  223. "%d > 2\n", pkt[0] & 0xff);
  224. if (cdp_compute_csum(pkt, len) != 0)
  225. return;
  226. pkt += 4;
  227. len -= 4;
  228. vlan = htons(-1);
  229. nvlan = htons(-1);
  230. while (len > 0) {
  231. if (len < 4)
  232. goto pkt_short;
  233. ss = (const ushort *)pkt;
  234. type = ntohs(ss[0]);
  235. tlen = ntohs(ss[1]);
  236. if (tlen > len)
  237. goto pkt_short;
  238. pkt += tlen;
  239. len -= tlen;
  240. ss += 2; /* point ss to the data of the TLV */
  241. tlen -= 4;
  242. switch (type) {
  243. case CDP_DEVICE_ID_TLV:
  244. break;
  245. case CDP_ADDRESS_TLV:
  246. break;
  247. case CDP_PORT_ID_TLV:
  248. break;
  249. case CDP_CAPABILITIES_TLV:
  250. break;
  251. case CDP_VERSION_TLV:
  252. break;
  253. case CDP_PLATFORM_TLV:
  254. break;
  255. case CDP_NATIVE_VLAN_TLV:
  256. nvlan = *ss;
  257. break;
  258. case CDP_APPLIANCE_VLAN_TLV:
  259. t = (const uchar *)ss;
  260. while (tlen > 0) {
  261. if (tlen < 3)
  262. goto pkt_short;
  263. ss = (const ushort *)(t + 1);
  264. #ifdef CONFIG_CDP_APPLIANCE_VLAN_TYPE
  265. if (t[0] == CONFIG_CDP_APPLIANCE_VLAN_TYPE)
  266. vlan = *ss;
  267. #else
  268. /* XXX will this work; dunno */
  269. vlan = ntohs(*ss);
  270. #endif
  271. t += 3; tlen -= 3;
  272. }
  273. break;
  274. case CDP_TRIGGER_TLV:
  275. break;
  276. case CDP_POWER_CONSUMPTION_TLV:
  277. break;
  278. case CDP_SYSNAME_TLV:
  279. break;
  280. case CDP_SYSOBJECT_TLV:
  281. break;
  282. case CDP_MANAGEMENT_ADDRESS_TLV:
  283. break;
  284. }
  285. }
  286. cdp_appliance_vlan = vlan;
  287. cdp_native_vlan = nvlan;
  288. cdp_ok = 1;
  289. return;
  290. pkt_short:
  291. printf("** CDP packet is too short\n");
  292. return;
  293. }
  294. void cdp_start(void)
  295. {
  296. printf("Using %s device\n", eth_get_name());
  297. cdp_seq = 0;
  298. cdp_ok = 0;
  299. cdp_native_vlan = htons(-1);
  300. cdp_appliance_vlan = htons(-1);
  301. net_set_timeout_handler(CDP_TIMEOUT, cdp_timeout_handler);
  302. cdp_send_trigger();
  303. }