cdp.c 7.6 KB

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