MnpVlan.c 22 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731
  1. /** @file
  2. VLAN Config Protocol implementation and VLAN packet process routine.
  3. Copyright (c) 2009 - 2018, Intel Corporation. All rights reserved.<BR>
  4. SPDX-License-Identifier: BSD-2-Clause-Patent
  5. **/
  6. #include "MnpImpl.h"
  7. #include "MnpVlan.h"
  8. VLAN_DEVICE_PATH mVlanDevicePathTemplate = {
  9. {
  10. MESSAGING_DEVICE_PATH,
  11. MSG_VLAN_DP,
  12. {
  13. (UINT8)(sizeof (VLAN_DEVICE_PATH)),
  14. (UINT8)((sizeof (VLAN_DEVICE_PATH)) >> 8)
  15. }
  16. },
  17. 0
  18. };
  19. EFI_VLAN_CONFIG_PROTOCOL mVlanConfigProtocolTemplate = {
  20. VlanConfigSet,
  21. VlanConfigFind,
  22. VlanConfigRemove
  23. };
  24. /**
  25. Create a child handle for the VLAN ID.
  26. @param[in] ImageHandle The driver image handle.
  27. @param[in] ControllerHandle Handle of device to bind driver to.
  28. @param[in] VlanId The VLAN ID.
  29. @param[out] Devicepath Pointer to returned device path for child handle.
  30. @return The handle of VLAN child or NULL if failed to create VLAN child.
  31. **/
  32. EFI_HANDLE
  33. MnpCreateVlanChild (
  34. IN EFI_HANDLE ImageHandle,
  35. IN EFI_HANDLE ControllerHandle,
  36. IN UINT16 VlanId,
  37. OUT EFI_DEVICE_PATH_PROTOCOL **Devicepath OPTIONAL
  38. )
  39. {
  40. EFI_HANDLE ChildHandle;
  41. VLAN_DEVICE_PATH VlanNode;
  42. EFI_DEVICE_PATH_PROTOCOL *ParentDevicePath;
  43. EFI_DEVICE_PATH_PROTOCOL *VlanDevicePath;
  44. EFI_STATUS Status;
  45. //
  46. // Try to get parent device path
  47. //
  48. Status = gBS->OpenProtocol (
  49. ControllerHandle,
  50. &gEfiDevicePathProtocolGuid,
  51. (VOID **)&ParentDevicePath,
  52. ImageHandle,
  53. ControllerHandle,
  54. EFI_OPEN_PROTOCOL_GET_PROTOCOL
  55. );
  56. if (EFI_ERROR (Status)) {
  57. return NULL;
  58. }
  59. //
  60. // Construct device path for child handle: MAC + VLAN
  61. //
  62. CopyMem (&VlanNode, &mVlanDevicePathTemplate, sizeof (VLAN_DEVICE_PATH));
  63. VlanNode.VlanId = VlanId;
  64. VlanDevicePath = AppendDevicePathNode (
  65. ParentDevicePath,
  66. (EFI_DEVICE_PATH_PROTOCOL *)&VlanNode
  67. );
  68. if (VlanDevicePath == NULL) {
  69. return NULL;
  70. }
  71. //
  72. // Create child VLAN handle by installing DevicePath protocol
  73. //
  74. ChildHandle = NULL;
  75. Status = gBS->InstallMultipleProtocolInterfaces (
  76. &ChildHandle,
  77. &gEfiDevicePathProtocolGuid,
  78. VlanDevicePath,
  79. NULL
  80. );
  81. if (EFI_ERROR (Status)) {
  82. FreePool (VlanDevicePath);
  83. return NULL;
  84. }
  85. if (Devicepath != NULL) {
  86. *Devicepath = VlanDevicePath;
  87. }
  88. return ChildHandle;
  89. }
  90. /**
  91. Remove VLAN tag from a packet.
  92. @param[in, out] MnpDeviceData Pointer to the mnp device context data.
  93. @param[in, out] Nbuf Pointer to the NET_BUF to remove VLAN tag.
  94. @param[out] VlanId Pointer to the returned VLAN ID.
  95. @retval TRUE VLAN tag is removed from this packet.
  96. @retval FALSE There is no VLAN tag in this packet.
  97. **/
  98. BOOLEAN
  99. MnpRemoveVlanTag (
  100. IN OUT MNP_DEVICE_DATA *MnpDeviceData,
  101. IN OUT NET_BUF *Nbuf,
  102. OUT UINT16 *VlanId
  103. )
  104. {
  105. UINT8 *Packet;
  106. UINTN ProtocolOffset;
  107. UINT16 ProtocolType;
  108. VLAN_TCI VlanTag;
  109. ProtocolOffset = MnpDeviceData->Snp->Mode->HwAddressSize * 2;
  110. //
  111. // Get the packet buffer.
  112. //
  113. Packet = NetbufGetByte (Nbuf, 0, NULL);
  114. ASSERT (Packet != NULL);
  115. //
  116. // Check whether this is VLAN tagged frame by Ether Type
  117. //
  118. *VlanId = 0;
  119. ProtocolType = NTOHS (*(UINT16 *)(Packet + ProtocolOffset));
  120. if (ProtocolType != ETHER_TYPE_VLAN) {
  121. //
  122. // Not a VLAN tagged frame
  123. //
  124. return FALSE;
  125. }
  126. VlanTag.Uint16 = NTOHS (*(UINT16 *)(Packet + ProtocolOffset + sizeof (ProtocolType)));
  127. *VlanId = VlanTag.Bits.Vid;
  128. //
  129. // Move hardware address (DA + SA) 4 bytes right to override VLAN tag
  130. //
  131. CopyMem (Packet + NET_VLAN_TAG_LEN, Packet, ProtocolOffset);
  132. //
  133. // Remove VLAN tag from the Nbuf
  134. //
  135. NetbufTrim (Nbuf, NET_VLAN_TAG_LEN, NET_BUF_HEAD);
  136. return TRUE;
  137. }
  138. /**
  139. Build the vlan packet to transmit from the TxData passed in.
  140. @param MnpServiceData Pointer to the mnp service context data.
  141. @param TxData Pointer to the transmit data containing the
  142. information to build the packet.
  143. @param ProtocolType Pointer to the Ethernet protocol type.
  144. @param Packet Pointer to record the address of the packet.
  145. @param Length Pointer to a UINT32 variable used to record the
  146. packet's length.
  147. **/
  148. VOID
  149. MnpInsertVlanTag (
  150. IN MNP_SERVICE_DATA *MnpServiceData,
  151. IN EFI_MANAGED_NETWORK_TRANSMIT_DATA *TxData,
  152. OUT UINT16 *ProtocolType,
  153. IN OUT UINT8 **Packet,
  154. IN OUT UINT32 *Length
  155. )
  156. {
  157. VLAN_TCI *VlanTci;
  158. UINT16 *Tpid;
  159. UINT16 *EtherType;
  160. MNP_DEVICE_DATA *MnpDeviceData;
  161. EFI_SIMPLE_NETWORK_MODE *SnpMode;
  162. MnpDeviceData = MnpServiceData->MnpDeviceData;
  163. SnpMode = MnpDeviceData->Snp->Mode;
  164. *ProtocolType = ETHER_TYPE_VLAN;
  165. *Length = *Length + NET_VLAN_TAG_LEN;
  166. *Packet = *Packet - NET_VLAN_TAG_LEN;
  167. Tpid = (UINT16 *)(*Packet + SnpMode->MediaHeaderSize - sizeof (*ProtocolType));
  168. VlanTci = (VLAN_TCI *)(UINTN)(Tpid + 1);
  169. if (TxData->HeaderLength != 0) {
  170. //
  171. // Media header is in packet, move DA+SA 4 bytes left
  172. //
  173. CopyMem (
  174. *Packet,
  175. *Packet + NET_VLAN_TAG_LEN,
  176. SnpMode->MediaHeaderSize - sizeof (*ProtocolType)
  177. );
  178. *Tpid = HTONS (ETHER_TYPE_VLAN);
  179. } else {
  180. //
  181. // Media header not in packet, VLAN TCI and original protocol type becomes payload
  182. //
  183. EtherType = (UINT16 *)(UINTN)(VlanTci + 1);
  184. *EtherType = HTONS (TxData->ProtocolType);
  185. }
  186. VlanTci->Bits.Vid = MnpServiceData->VlanId;
  187. VlanTci->Bits.Cfi = VLAN_TCI_CFI_CANONICAL_MAC;
  188. VlanTci->Bits.Priority = MnpServiceData->Priority;
  189. VlanTci->Uint16 = HTONS (VlanTci->Uint16);
  190. }
  191. /**
  192. Check VLAN configuration variable and delete the duplicative content if has identical Vlan ID.
  193. @param[in] MnpDeviceData Pointer to the MNP device context data.
  194. @param[in] Buffer Pointer to the buffer contains the array of VLAN_TCI.
  195. @param[in] NumberOfVlan Pointer to number of VLAN.
  196. @param[out] NewNumberOfVlan Pointer to number of unique VLAN.
  197. @retval EFI_SUCCESS The VLAN variable is successfully checked.
  198. @retval EFI_OUT_OF_RESOURCES There is not enough resource to set the configuration.
  199. **/
  200. EFI_STATUS
  201. MnpCheckVlanVariable (
  202. IN MNP_DEVICE_DATA *MnpDeviceData,
  203. IN VLAN_TCI *Buffer,
  204. IN UINTN NumberOfVlan,
  205. OUT UINTN *NewNumberOfVlan
  206. )
  207. {
  208. UINTN Index;
  209. UINTN Index2;
  210. UINTN Count;
  211. BOOLEAN FoundDuplicateItem;
  212. EFI_STATUS Status;
  213. Count = 0;
  214. FoundDuplicateItem = FALSE;
  215. Status = EFI_SUCCESS;
  216. for (Index = 0; Index < NumberOfVlan; Index++) {
  217. for (Index2 = Index + 1; Index2 < NumberOfVlan; Index2++) {
  218. if (Buffer[Index].Bits.Vid == Buffer[Index2].Bits.Vid) {
  219. FoundDuplicateItem = TRUE;
  220. Count++;
  221. break;
  222. }
  223. }
  224. if (FoundDuplicateItem) {
  225. for (Index2 = Index +1; Index2 < NumberOfVlan; Index++, Index2++) {
  226. CopyMem (Buffer + Index, Buffer + Index2, sizeof (VLAN_TCI));
  227. }
  228. }
  229. FoundDuplicateItem = FALSE;
  230. }
  231. *NewNumberOfVlan = NumberOfVlan - Count;
  232. if (Count != 0) {
  233. Status = MnpSetVlanVariable (MnpDeviceData, *NewNumberOfVlan, Buffer);
  234. }
  235. return Status;
  236. }
  237. /**
  238. Get VLAN configuration variable.
  239. @param[in] MnpDeviceData Pointer to the MNP device context data.
  240. @param[out] NumberOfVlan Pointer to number of VLAN to be returned.
  241. @param[out] VlanVariable Pointer to the buffer to return requested
  242. array of VLAN_TCI.
  243. @retval EFI_SUCCESS The array of VLAN_TCI was returned in VlanVariable
  244. and number of VLAN was returned in NumberOfVlan.
  245. @retval EFI_NOT_FOUND VLAN configuration variable not found.
  246. @retval EFI_OUT_OF_RESOURCES There is not enough pool memory to store the configuration.
  247. **/
  248. EFI_STATUS
  249. MnpGetVlanVariable (
  250. IN MNP_DEVICE_DATA *MnpDeviceData,
  251. OUT UINTN *NumberOfVlan,
  252. OUT VLAN_TCI **VlanVariable
  253. )
  254. {
  255. UINTN BufferSize;
  256. EFI_STATUS Status;
  257. VLAN_TCI *Buffer;
  258. UINTN NewNumberOfVlan;
  259. //
  260. // Get VLAN configuration from EFI Variable
  261. //
  262. Buffer = NULL;
  263. BufferSize = 0;
  264. Status = gRT->GetVariable (
  265. MnpDeviceData->MacString,
  266. &gEfiVlanConfigProtocolGuid,
  267. NULL,
  268. &BufferSize,
  269. NULL
  270. );
  271. if (Status != EFI_BUFFER_TOO_SMALL) {
  272. return EFI_NOT_FOUND;
  273. }
  274. //
  275. // Allocate buffer to read the variable
  276. //
  277. Buffer = AllocateZeroPool (BufferSize);
  278. if (Buffer == NULL) {
  279. return EFI_OUT_OF_RESOURCES;
  280. }
  281. Status = gRT->GetVariable (
  282. MnpDeviceData->MacString,
  283. &gEfiVlanConfigProtocolGuid,
  284. NULL,
  285. &BufferSize,
  286. Buffer
  287. );
  288. if (EFI_ERROR (Status)) {
  289. FreePool (Buffer);
  290. return Status;
  291. }
  292. Status = MnpCheckVlanVariable (MnpDeviceData, Buffer, BufferSize / sizeof (VLAN_TCI), &NewNumberOfVlan);
  293. if (!EFI_ERROR (Status)) {
  294. *NumberOfVlan = NewNumberOfVlan;
  295. *VlanVariable = Buffer;
  296. }
  297. return Status;
  298. }
  299. /**
  300. Set VLAN configuration variable.
  301. @param[in] MnpDeviceData Pointer to the MNP device context data.
  302. @param[in] NumberOfVlan Number of VLAN in array VlanVariable.
  303. @param[in] VlanVariable Pointer to array of VLAN_TCI.
  304. @retval EFI_SUCCESS The VLAN variable is successfully set.
  305. @retval EFI_OUT_OF_RESOURCES There is not enough resource to set the configuration.
  306. **/
  307. EFI_STATUS
  308. MnpSetVlanVariable (
  309. IN MNP_DEVICE_DATA *MnpDeviceData,
  310. IN UINTN NumberOfVlan,
  311. IN VLAN_TCI *VlanVariable
  312. )
  313. {
  314. return gRT->SetVariable (
  315. MnpDeviceData->MacString,
  316. &gEfiVlanConfigProtocolGuid,
  317. EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS,
  318. NumberOfVlan * sizeof (VLAN_TCI),
  319. VlanVariable
  320. );
  321. }
  322. /**
  323. Create a VLAN device or modify the configuration parameter of an
  324. already-configured VLAN.
  325. The Set() function is used to create a new VLAN device or change the VLAN
  326. configuration parameters. If the VlanId hasn't been configured in the
  327. physical Ethernet device, a new VLAN device will be created. If a VLAN with
  328. this VlanId is already configured, then related configuration will be updated
  329. as the input parameters.
  330. If VlanId is zero, the VLAN device will send and receive untagged frames.
  331. Otherwise, the VLAN device will send and receive VLAN-tagged frames containing the VlanId.
  332. If VlanId is out of scope of (0-4094), EFI_INVALID_PARAMETER is returned.
  333. If Priority is out of the scope of (0-7), then EFI_INVALID_PARAMETER is returned.
  334. If there is not enough system memory to perform the registration, then
  335. EFI_OUT_OF_RESOURCES is returned.
  336. @param[in] This Points to the EFI_VLAN_CONFIG_PROTOCOL.
  337. @param[in] VlanId A unique identifier (1-4094) of the VLAN which is being created
  338. or modified, or zero (0).
  339. @param[in] Priority 3 bit priority in VLAN header. Priority 0 is default value. If
  340. VlanId is zero (0), Priority is ignored.
  341. @retval EFI_SUCCESS The VLAN is successfully configured.
  342. @retval EFI_INVALID_PARAMETER One or more of following conditions is TRUE:
  343. - This is NULL.
  344. - VlanId is an invalid VLAN Identifier.
  345. - Priority is invalid.
  346. @retval EFI_OUT_OF_RESOURCES There is not enough system memory to perform the registration.
  347. **/
  348. EFI_STATUS
  349. EFIAPI
  350. VlanConfigSet (
  351. IN EFI_VLAN_CONFIG_PROTOCOL *This,
  352. IN UINT16 VlanId,
  353. IN UINT8 Priority
  354. )
  355. {
  356. EFI_STATUS Status;
  357. MNP_DEVICE_DATA *MnpDeviceData;
  358. MNP_SERVICE_DATA *MnpServiceData;
  359. VLAN_TCI *OldVariable;
  360. VLAN_TCI *NewVariable;
  361. UINTN NumberOfVlan;
  362. UINTN Index;
  363. BOOLEAN IsAdd;
  364. LIST_ENTRY *Entry;
  365. if ((This == NULL) || (VlanId > 4094) || (Priority > 7)) {
  366. return EFI_INVALID_PARAMETER;
  367. }
  368. IsAdd = FALSE;
  369. MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
  370. if (MnpDeviceData->NumberOfVlan == 0) {
  371. //
  372. // No existing VLAN, this is the first VLAN to add
  373. //
  374. IsAdd = TRUE;
  375. Entry = GetFirstNode (&MnpDeviceData->ServiceList);
  376. MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
  377. if (VlanId != 0) {
  378. //
  379. // VlanId is not 0, need destroy the default MNP service data
  380. //
  381. Status = MnpDestroyServiceChild (MnpServiceData);
  382. if (EFI_ERROR (Status)) {
  383. return Status;
  384. }
  385. Status = MnpDestroyServiceData (MnpServiceData);
  386. if (EFI_ERROR (Status)) {
  387. return Status;
  388. }
  389. //
  390. // Create a new MNP service data for this VLAN
  391. //
  392. MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
  393. if (MnpServiceData == NULL) {
  394. return EFI_OUT_OF_RESOURCES;
  395. }
  396. }
  397. } else {
  398. //
  399. // Try to find VlanId in existing VLAN list
  400. //
  401. MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
  402. if (MnpServiceData == NULL) {
  403. //
  404. // VlanId not found, create a new MNP service data
  405. //
  406. IsAdd = TRUE;
  407. MnpServiceData = MnpCreateServiceData (MnpDeviceData, VlanId, Priority);
  408. if (MnpServiceData == NULL) {
  409. return EFI_OUT_OF_RESOURCES;
  410. }
  411. }
  412. }
  413. MnpServiceData->VlanId = VlanId;
  414. MnpServiceData->Priority = Priority;
  415. if (IsAdd) {
  416. MnpDeviceData->NumberOfVlan++;
  417. }
  418. //
  419. // Update VLAN configuration variable
  420. //
  421. OldVariable = NULL;
  422. NewVariable = NULL;
  423. NumberOfVlan = 0;
  424. MnpGetVlanVariable (MnpDeviceData, &NumberOfVlan, &OldVariable);
  425. if (IsAdd) {
  426. //
  427. // VLAN not exist - add
  428. //
  429. NewVariable = AllocateZeroPool ((NumberOfVlan + 1) * sizeof (VLAN_TCI));
  430. if (NewVariable == NULL) {
  431. Status = EFI_OUT_OF_RESOURCES;
  432. goto Exit;
  433. }
  434. if (OldVariable != NULL) {
  435. CopyMem (NewVariable, OldVariable, NumberOfVlan * sizeof (VLAN_TCI));
  436. }
  437. Index = NumberOfVlan++;
  438. } else {
  439. //
  440. // VLAN already exist - update
  441. //
  442. for (Index = 0; Index < NumberOfVlan; Index++) {
  443. if (OldVariable[Index].Bits.Vid == VlanId) {
  444. break;
  445. }
  446. }
  447. ASSERT (Index < NumberOfVlan);
  448. NewVariable = OldVariable;
  449. OldVariable = NULL;
  450. }
  451. NewVariable[Index].Bits.Vid = VlanId;
  452. NewVariable[Index].Bits.Priority = Priority;
  453. Status = MnpSetVlanVariable (MnpDeviceData, NumberOfVlan, NewVariable);
  454. FreePool (NewVariable);
  455. Exit:
  456. if (OldVariable != NULL) {
  457. FreePool (OldVariable);
  458. }
  459. return Status;
  460. }
  461. /**
  462. Find configuration information for specified VLAN or all configured VLANs.
  463. The Find() function is used to find the configuration information for matching
  464. VLAN and allocate a buffer into which those entries are copied.
  465. @param[in] This Points to the EFI_VLAN_CONFIG_PROTOCOL.
  466. @param[in] VlanId Pointer to VLAN identifier. Set to NULL to find all
  467. configured VLANs.
  468. @param[out] NumberOfVlan The number of VLANs which is found by the specified criteria.
  469. @param[out] Entries The buffer which receive the VLAN configuration.
  470. @retval EFI_SUCCESS The VLAN is successfully found.
  471. @retval EFI_INVALID_PARAMETER One or more of following conditions is TRUE:
  472. - This is NULL.
  473. - Specified VlanId is invalid.
  474. @retval EFI_NOT_FOUND No matching VLAN is found.
  475. **/
  476. EFI_STATUS
  477. EFIAPI
  478. VlanConfigFind (
  479. IN EFI_VLAN_CONFIG_PROTOCOL *This,
  480. IN UINT16 *VlanId OPTIONAL,
  481. OUT UINT16 *NumberOfVlan,
  482. OUT EFI_VLAN_FIND_DATA **Entries
  483. )
  484. {
  485. MNP_DEVICE_DATA *MnpDeviceData;
  486. MNP_SERVICE_DATA *MnpServiceData;
  487. LIST_ENTRY *Entry;
  488. EFI_VLAN_FIND_DATA *VlanData;
  489. if ((This == NULL) || ((VlanId != NULL) && (*VlanId > 4094)) || (NumberOfVlan == NULL) || (Entries == NULL)) {
  490. return EFI_INVALID_PARAMETER;
  491. }
  492. *NumberOfVlan = 0;
  493. *Entries = NULL;
  494. MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
  495. if (MnpDeviceData->NumberOfVlan == 0) {
  496. return EFI_NOT_FOUND;
  497. }
  498. if (VlanId == NULL) {
  499. //
  500. // Return all current VLAN configuration
  501. //
  502. *NumberOfVlan = (UINT16)MnpDeviceData->NumberOfVlan;
  503. VlanData = AllocateZeroPool (*NumberOfVlan * sizeof (EFI_VLAN_FIND_DATA));
  504. if (VlanData == NULL) {
  505. return EFI_OUT_OF_RESOURCES;
  506. }
  507. *Entries = VlanData;
  508. NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
  509. MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
  510. VlanData->VlanId = MnpServiceData->VlanId;
  511. VlanData->Priority = MnpServiceData->Priority;
  512. VlanData++;
  513. }
  514. return EFI_SUCCESS;
  515. }
  516. //
  517. // VlanId is specified, try to find it in current VLAN list
  518. //
  519. MnpServiceData = MnpFindServiceData (MnpDeviceData, *VlanId);
  520. if (MnpServiceData == NULL) {
  521. return EFI_NOT_FOUND;
  522. }
  523. VlanData = AllocateZeroPool (sizeof (EFI_VLAN_FIND_DATA));
  524. if (VlanData == NULL) {
  525. return EFI_OUT_OF_RESOURCES;
  526. }
  527. VlanData->VlanId = MnpServiceData->VlanId;
  528. VlanData->Priority = MnpServiceData->Priority;
  529. *NumberOfVlan = 1;
  530. *Entries = VlanData;
  531. return EFI_SUCCESS;
  532. }
  533. /**
  534. Remove the configured VLAN device.
  535. The Remove() function is used to remove the specified VLAN device.
  536. If the VlanId is out of the scope of (0-4094), EFI_INVALID_PARAMETER is returned.
  537. If specified VLAN hasn't been previously configured, EFI_NOT_FOUND is returned.
  538. @param[in] This Points to the EFI_VLAN_CONFIG_PROTOCOL.
  539. @param[in] VlanId Identifier (0-4094) of the VLAN to be removed.
  540. @retval EFI_SUCCESS The VLAN is successfully removed.
  541. @retval EFI_INVALID_PARAMETER One or more of following conditions is TRUE:
  542. - This is NULL.
  543. - VlanId is an invalid parameter.
  544. @retval EFI_NOT_FOUND The to-be-removed VLAN does not exist.
  545. **/
  546. EFI_STATUS
  547. EFIAPI
  548. VlanConfigRemove (
  549. IN EFI_VLAN_CONFIG_PROTOCOL *This,
  550. IN UINT16 VlanId
  551. )
  552. {
  553. EFI_STATUS Status;
  554. MNP_DEVICE_DATA *MnpDeviceData;
  555. MNP_SERVICE_DATA *MnpServiceData;
  556. LIST_ENTRY *Entry;
  557. VLAN_TCI *VlanVariable;
  558. VLAN_TCI *VlanData;
  559. if ((This == NULL) || (VlanId > 4094)) {
  560. return EFI_INVALID_PARAMETER;
  561. }
  562. MnpDeviceData = MNP_DEVICE_DATA_FROM_THIS (This);
  563. if (MnpDeviceData->NumberOfVlan == 0) {
  564. return EFI_NOT_FOUND;
  565. }
  566. //
  567. // Try to find the VlanId
  568. //
  569. MnpServiceData = MnpFindServiceData (MnpDeviceData, VlanId);
  570. if (MnpServiceData == NULL) {
  571. return EFI_NOT_FOUND;
  572. }
  573. MnpDeviceData->NumberOfVlan--;
  574. if ((VlanId != 0) || (MnpDeviceData->NumberOfVlan != 0)) {
  575. //
  576. // If VlanId is not 0 or VlanId is 0 and it is not the last VLAN to remove,
  577. // destroy its MNP service data
  578. //
  579. Status = MnpDestroyServiceChild (MnpServiceData);
  580. if (EFI_ERROR (Status)) {
  581. return Status;
  582. }
  583. Status = MnpDestroyServiceData (MnpServiceData);
  584. if (EFI_ERROR (Status)) {
  585. return Status;
  586. }
  587. }
  588. if ((VlanId != 0) && (MnpDeviceData->NumberOfVlan == 0)) {
  589. //
  590. // This is the last VLAN to be removed, restore the default MNP service data
  591. //
  592. MnpServiceData = MnpCreateServiceData (MnpDeviceData, 0, 0);
  593. if (MnpServiceData == NULL) {
  594. return EFI_OUT_OF_RESOURCES;
  595. }
  596. }
  597. //
  598. // Update VLAN configuration variable
  599. //
  600. VlanVariable = NULL;
  601. if (MnpDeviceData->NumberOfVlan != 0) {
  602. VlanVariable = AllocatePool (MnpDeviceData->NumberOfVlan * sizeof (VLAN_TCI));
  603. if (VlanVariable == NULL) {
  604. return EFI_OUT_OF_RESOURCES;
  605. }
  606. VlanData = VlanVariable;
  607. NET_LIST_FOR_EACH (Entry, &MnpDeviceData->ServiceList) {
  608. MnpServiceData = MNP_SERVICE_DATA_FROM_LINK (Entry);
  609. VlanData->Bits.Vid = MnpServiceData->VlanId;
  610. VlanData->Bits.Priority = MnpServiceData->Priority;
  611. VlanData++;
  612. }
  613. }
  614. Status = MnpSetVlanVariable (MnpDeviceData, MnpDeviceData->NumberOfVlan, VlanVariable);
  615. if (VlanVariable != NULL) {
  616. FreePool (VlanVariable);
  617. }
  618. return Status;
  619. }