[linux-block.git] / drivers / infiniband / hw / mthca / mthca_mcg.c

/*
 * Copyright (c) 2004 Topspin Communications.  All rights reserved.
 *
 * This software is available to you under a choice of one of two
 * licenses.  You may choose to be licensed under the terms of the GNU
 * General Public License (GPL) Version 2, available from the file
 * COPYING in the main directory of this source tree, or the
 * OpenIB.org BSD license below:
 *
 *     Redistribution and use in source and binary forms, with or
 *     without modification, are permitted provided that the following
 *     conditions are met:
 *
 *      - Redistributions of source code must retain the above
 *        copyright notice, this list of conditions and the following
 *        disclaimer.
 *
 *      - Redistributions in binary form must reproduce the above
 *        copyright notice, this list of conditions and the following
 *        disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * $Id: mthca_mcg.c 1349 2004-12-16 21:09:43Z roland $
 */

#include <linux/init.h>
#include <linux/string.h>
#include <linux/slab.h>

#include "mthca_dev.h"
#include "mthca_cmd.h"

struct mthca_mgm {
	__be32 next_gid_index;
	u32    reserved[3];
	u8     gid[16];
	__be32 qp[MTHCA_QP_PER_MGM];
};

static const u8 zero_gid[16];	/* automatically initialized to 0 */

/*
 * Caller must hold MCG table semaphore.  gid and mgm parameters must
 * be properly aligned for command interface.
 *
 *  Returns 0 unless a firmware command error occurs.
 *
 * If GID is found in MGM or MGM is empty, *index = *hash, *prev = -1
 * and *mgm holds MGM entry.
 *
 * if GID is found in AMGM, *index = index in AMGM, *prev = index of
 * previous entry in hash chain and *mgm holds AMGM entry.
 *
 * If no AMGM exists for given gid, *index = -1, *prev = index of last
 * entry in hash chain and *mgm holds end of hash chain.
 */
static int find_mgm(struct mthca_dev *dev,
		    u8 *gid, struct mthca_mailbox *mgm_mailbox,
		    u16 *hash, int *prev, int *index)
{
	struct mthca_mailbox *mailbox;
	struct mthca_mgm *mgm = mgm_mailbox->buf;
	u8 *mgid;
	int err;
	u8 status;

	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
	if (IS_ERR(mailbox))
		return -ENOMEM;
	mgid = mailbox->buf;

	memcpy(mgid, gid, 16);

	err = mthca_MGID_HASH(dev, mailbox, hash, &status);
	if (err)
		goto out;
	if (status) {
		mthca_err(dev, "MGID_HASH returned status %02x\n", status);
		err = -EINVAL;
		goto out;
	}

	if (0)
		mthca_dbg(dev, "Hash for %04x:%04x:%04x:%04x:"
			  "%04x:%04x:%04x:%04x is %04x\n",
			  be16_to_cpu(((__be16 *) gid)[0]),
			  be16_to_cpu(((__be16 *) gid)[1]),
			  be16_to_cpu(((__be16 *) gid)[2]),
			  be16_to_cpu(((__be16 *) gid)[3]),
			  be16_to_cpu(((__be16 *) gid)[4]),
			  be16_to_cpu(((__be16 *) gid)[5]),
			  be16_to_cpu(((__be16 *) gid)[6]),
			  be16_to_cpu(((__be16 *) gid)[7]),
			  *hash);

	*index = *hash;
	*prev  = -1;

	do {
		err = mthca_READ_MGM(dev, *index, mgm_mailbox, &status);
		if (err)
			goto out;
		if (status) {
			mthca_err(dev, "READ_MGM returned status %02x\n", status);
			err = -EINVAL;
			goto out;
		}

		if (!memcmp(mgm->gid, zero_gid, 16)) {
			if (*index != *hash) {
				mthca_err(dev, "Found zero MGID in AMGM.\n");
				err = -EINVAL;
			}
			goto out;
		}

		if (!memcmp(mgm->gid, gid, 16))
			goto out;

		*prev = *index;
		*index = be32_to_cpu(mgm->next_gid_index) >> 6;
	} while (*index);

	*index = -1;

 out:
	mthca_free_mailbox(dev, mailbox);
	return err;
}

int mthca_multicast_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
{
	struct mthca_dev *dev = to_mdev(ibqp->device);
	struct mthca_mailbox *mailbox;
	struct mthca_mgm *mgm;
	u16 hash;
	int index, prev;
	int link = 0;
	int i;
	int err;
	u8 status;

	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
	if (IS_ERR(mailbox))
		return PTR_ERR(mailbox);
	mgm = mailbox->buf;

	if (down_interruptible(&dev->mcg_table.sem)) {
		err = -EINTR;
		goto err_sem;
	}

	err = find_mgm(dev, gid->raw, mailbox, &hash, &prev, &index);
	if (err)
		goto out;

	if (index != -1) {
		if (!memcmp(mgm->gid, zero_gid, 16))
			memcpy(mgm->gid, gid->raw, 16);
	} else {
		link = 1;

		index = mthca_alloc(&dev->mcg_table.alloc);
		if (index == -1) {
			mthca_err(dev, "No AMGM entries left\n");
			err = -ENOMEM;
			goto out;
		}

		err = mthca_READ_MGM(dev, index, mailbox, &status);
		if (err)
			goto out;
		if (status) {
			mthca_err(dev, "READ_MGM returned status %02x\n", status);
			err = -EINVAL;
			goto out;
		}
		memset(mgm, 0, sizeof *mgm);
		memcpy(mgm->gid, gid->raw, 16);
	}

	for (i = 0; i < MTHCA_QP_PER_MGM; ++i)
		if (mgm->qp[i] == cpu_to_be32(ibqp->qp_num | (1 << 31))) {
			mthca_dbg(dev, "QP %06x already a member of MGM\n", 
				  ibqp->qp_num);
			err = 0;
			goto out;
		} else if (!(mgm->qp[i] & cpu_to_be32(1 << 31))) {
			mgm->qp[i] = cpu_to_be32(ibqp->qp_num | (1 << 31));
			break;
		}

	if (i == MTHCA_QP_PER_MGM) {
		mthca_err(dev, "MGM at index %x is full.\n", index);
		err = -ENOMEM;
		goto out;
	}

	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
	if (err)
		goto out;
	if (status) {
		mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
		err = -EINVAL;
		goto out;
	}

	if (!link)
		goto out;

	err = mthca_READ_MGM(dev, prev, mailbox, &status);
	if (err)
		goto out;
	if (status) {
		mthca_err(dev, "READ_MGM returned status %02x\n", status);
		err = -EINVAL;
		goto out;
	}

	mgm->next_gid_index = cpu_to_be32(index << 6);

	err = mthca_WRITE_MGM(dev, prev, mailbox, &status);
	if (err)
		goto out;
	if (status) {
		mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
		err = -EINVAL;
	}

 out:
	if (err && link && index != -1) {
		BUG_ON(index < dev->limits.num_mgms);
		mthca_free(&dev->mcg_table.alloc, index);
	}
	up(&dev->mcg_table.sem);
 err_sem:
	mthca_free_mailbox(dev, mailbox);
	return err;
}

int mthca_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
{
	struct mthca_dev *dev = to_mdev(ibqp->device);
	struct mthca_mailbox *mailbox;
	struct mthca_mgm *mgm;
	u16 hash;
	int prev, index;
	int i, loc;
	int err;
	u8 status;

	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
	if (IS_ERR(mailbox))
		return PTR_ERR(mailbox);
	mgm = mailbox->buf;

	if (down_interruptible(&dev->mcg_table.sem)) {
		err = -EINTR;
		goto err_sem;
	}

	err = find_mgm(dev, gid->raw, mailbox, &hash, &prev, &index);
	if (err)
		goto out;

	if (index == -1) {
		mthca_err(dev, "MGID %04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x "
			  "not found\n",
			  be16_to_cpu(((__be16 *) gid->raw)[0]),
			  be16_to_cpu(((__be16 *) gid->raw)[1]),
			  be16_to_cpu(((__be16 *) gid->raw)[2]),
			  be16_to_cpu(((__be16 *) gid->raw)[3]),
			  be16_to_cpu(((__be16 *) gid->raw)[4]),
			  be16_to_cpu(((__be16 *) gid->raw)[5]),
			  be16_to_cpu(((__be16 *) gid->raw)[6]),
			  be16_to_cpu(((__be16 *) gid->raw)[7]));
		err = -EINVAL;
		goto out;
	}

	for (loc = -1, i = 0; i < MTHCA_QP_PER_MGM; ++i) {
		if (mgm->qp[i] == cpu_to_be32(ibqp->qp_num | (1 << 31)))
			loc = i;
		if (!(mgm->qp[i] & cpu_to_be32(1 << 31)))
			break;
	}

	if (loc == -1) {
		mthca_err(dev, "QP %06x not found in MGM\n", ibqp->qp_num);
		err = -EINVAL;
		goto out;
	}

	mgm->qp[loc]   = mgm->qp[i - 1];
	mgm->qp[i - 1] = 0;

	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
	if (err)
		goto out;
	if (status) {
		mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
		err = -EINVAL;
		goto out;
	}

	if (i != 1)
		goto out;

	if (prev == -1) {
		/* Remove entry from MGM */
		int amgm_index_to_free = be32_to_cpu(mgm->next_gid_index) >> 6;
		if (amgm_index_to_free) {
			err = mthca_READ_MGM(dev, amgm_index_to_free,
					     mailbox, &status);
			if (err)
				goto out;
			if (status) {
				mthca_err(dev, "READ_MGM returned status %02x\n",
					  status);
				err = -EINVAL;
				goto out;
			}
		} else
			memset(mgm->gid, 0, 16);

		err = mthca_WRITE_MGM(dev, index, mailbox, &status);
		if (err)
			goto out;
		if (status) {
			mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
			err = -EINVAL;
			goto out;
		}
		if (amgm_index_to_free) {
			BUG_ON(amgm_index_to_free < dev->limits.num_mgms);
			mthca_free(&dev->mcg_table.alloc, amgm_index_to_free);
		}
	} else {
		/* Remove entry from AMGM */
		int curr_next_index = be32_to_cpu(mgm->next_gid_index) >> 6;
		err = mthca_READ_MGM(dev, prev, mailbox, &status);
		if (err)
			goto out;
		if (status) {
			mthca_err(dev, "READ_MGM returned status %02x\n", status);
			err = -EINVAL;
			goto out;
		}

		mgm->next_gid_index = cpu_to_be32(curr_next_index << 6);

		err = mthca_WRITE_MGM(dev, prev, mailbox, &status);
		if (err)
			goto out;
		if (status) {
			mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
			err = -EINVAL;
			goto out;
		}
		BUG_ON(index < dev->limits.num_mgms);
		mthca_free(&dev->mcg_table.alloc, index);
	}

 out:
	up(&dev->mcg_table.sem);
 err_sem:
	mthca_free_mailbox(dev, mailbox);
	return err;
}

int __devinit mthca_init_mcg_table(struct mthca_dev *dev)
{
	int err;
	int table_size = dev->limits.num_mgms + dev->limits.num_amgms;

	err = mthca_alloc_init(&dev->mcg_table.alloc,
			       table_size,
			       table_size - 1,
			       dev->limits.num_mgms);
	if (err)
		return err;

	init_MUTEX(&dev->mcg_table.sem);

	return 0;
}

void __devexit mthca_cleanup_mcg_table(struct mthca_dev *dev)
{
	mthca_alloc_cleanup(&dev->mcg_table.alloc);
}
Commit	Line	Data
1da177e4 LT	1	/*
	2	* Copyright (c) 2004 Topspin Communications. All rights reserved.
	3	*
	4	* This software is available to you under a choice of one of two
	5	* licenses. You may choose to be licensed under the terms of the GNU
	6	* General Public License (GPL) Version 2, available from the file
	7	* COPYING in the main directory of this source tree, or the
	8	* OpenIB.org BSD license below:
	9	*
	10	* Redistribution and use in source and binary forms, with or
	11	* without modification, are permitted provided that the following
	12	* conditions are met:
	13	*
	14	* - Redistributions of source code must retain the above
	15	* copyright notice, this list of conditions and the following
	16	* disclaimer.
	17	*
	18	* - Redistributions in binary form must reproduce the above
	19	* copyright notice, this list of conditions and the following
	20	* disclaimer in the documentation and/or other materials
	21	* provided with the distribution.
	22	*
	23	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
	24	* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
	25	* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
	26	* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
	27	* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
	28	* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
	29	* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
	30	* SOFTWARE.
	31	*
	32	* $Id: mthca_mcg.c 1349 2004-12-16 21:09:43Z roland $
	33	*/
	34
	35	#include <linux/init.h>
4e57b681 TS	36	#include <linux/string.h>
4e57b681 TS	37	#include <linux/slab.h>
1da177e4 LT	38
	39	#include "mthca_dev.h"
	40	#include "mthca_cmd.h"
	41
1da177e4	42	struct mthca_mgm {
97f52eb4 SH	43	__be32 next_gid_index;
	44	u32 reserved[3];
	45	u8 gid[16];
	46	__be32 qp[MTHCA_QP_PER_MGM];
1da177e4 LT	47	};
	48
	49	static const u8 zero_gid[16]; /* automatically initialized to 0 */
	50
	51	/*
	52	* Caller must hold MCG table semaphore. gid and mgm parameters must
	53	* be properly aligned for command interface.
	54	*
	55	* Returns 0 unless a firmware command error occurs.
	56	*
	57	* If GID is found in MGM or MGM is empty, index = hash, *prev = -1
	58	* and *mgm holds MGM entry.
	59	*
	60	* if GID is found in AMGM, index = index in AMGM, prev = index of
	61	* previous entry in hash chain and *mgm holds AMGM entry.
	62	*
	63	* If no AMGM exists for given gid, index = -1, prev = index of last
	64	* entry in hash chain and *mgm holds end of hash chain.
	65	*/
	66	static int find_mgm(struct mthca_dev *dev,
ed878458	67	u8 gid, struct mthca_mailbox mgm_mailbox,
1da177e4 LT	68	u16 hash, int prev, int *index)
1da177e4 LT	69	{
ed878458 RD	70	struct mthca_mailbox *mailbox;
ed878458 RD	71	struct mthca_mgm *mgm = mgm_mailbox->buf;
1da177e4 LT	72	u8 *mgid;
	73	int err;
	74	u8 status;
	75
ed878458 RD	76	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
ed878458 RD	77	if (IS_ERR(mailbox))
1da177e4	78	return -ENOMEM;
ed878458	79	mgid = mailbox->buf;
1da177e4 LT	80
	81	memcpy(mgid, gid, 16);
	82
ed878458	83	err = mthca_MGID_HASH(dev, mailbox, hash, &status);
1da177e4 LT	84	if (err)
	85	goto out;
	86	if (status) {
	87	mthca_err(dev, "MGID_HASH returned status %02x\n", status);
	88	err = -EINVAL;
	89	goto out;
	90	}
	91
	92	if (0)
	93	mthca_dbg(dev, "Hash for %04x:%04x:%04x:%04x:"
	94	"%04x:%04x:%04x:%04x is %04x\n",
97f52eb4 SH	95	be16_to_cpu(((__be16 *) gid)[0]),
	96	be16_to_cpu(((__be16 *) gid)[1]),
	97	be16_to_cpu(((__be16 *) gid)[2]),
	98	be16_to_cpu(((__be16 *) gid)[3]),
	99	be16_to_cpu(((__be16 *) gid)[4]),
	100	be16_to_cpu(((__be16 *) gid)[5]),
	101	be16_to_cpu(((__be16 *) gid)[6]),
	102	be16_to_cpu(((__be16 *) gid)[7]),
1da177e4 LT	103	*hash);
	104
	105	index = hash;
	106	*prev = -1;
	107
	108	do {
ed878458	109	err = mthca_READ_MGM(dev, *index, mgm_mailbox, &status);
1da177e4 LT	110	if (err)
	111	goto out;
	112	if (status) {
	113	mthca_err(dev, "READ_MGM returned status %02x\n", status);
5ceb7455 JM	114	err = -EINVAL;
5ceb7455 JM	115	goto out;
1da177e4 LT	116	}
	117
	118	if (!memcmp(mgm->gid, zero_gid, 16)) {
	119	if (index != hash) {
	120	mthca_err(dev, "Found zero MGID in AMGM.\n");
	121	err = -EINVAL;
	122	}
	123	goto out;
	124	}
	125
	126	if (!memcmp(mgm->gid, gid, 16))
	127	goto out;
	128
	129	prev = index;
5ceb7455	130	*index = be32_to_cpu(mgm->next_gid_index) >> 6;
1da177e4 LT	131	} while (*index);
	132
	133	*index = -1;
	134
	135	out:
ed878458	136	mthca_free_mailbox(dev, mailbox);
1da177e4 LT	137	return err;
	138	}
	139
	140	int mthca_multicast_attach(struct ib_qp ibqp, union ib_gid gid, u16 lid)
	141	{
	142	struct mthca_dev *dev = to_mdev(ibqp->device);
ed878458	143	struct mthca_mailbox *mailbox;
1da177e4 LT	144	struct mthca_mgm *mgm;
	145	u16 hash;
	146	int index, prev;
	147	int link = 0;
	148	int i;
	149	int err;
	150	u8 status;
	151
ed878458 RD	152	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
	153	if (IS_ERR(mailbox))
	154	return PTR_ERR(mailbox);
	155	mgm = mailbox->buf;
1da177e4	156
5ceb7455 JM	157	if (down_interruptible(&dev->mcg_table.sem)) {
	158	err = -EINTR;
	159	goto err_sem;
	160	}
1da177e4	161
ed878458	162	err = find_mgm(dev, gid->raw, mailbox, &hash, &prev, &index);
1da177e4 LT	163	if (err)
	164	goto out;
	165
	166	if (index != -1) {
	167	if (!memcmp(mgm->gid, zero_gid, 16))
	168	memcpy(mgm->gid, gid->raw, 16);
	169	} else {
	170	link = 1;
	171
	172	index = mthca_alloc(&dev->mcg_table.alloc);
	173	if (index == -1) {
	174	mthca_err(dev, "No AMGM entries left\n");
	175	err = -ENOMEM;
	176	goto out;
	177	}
	178
ed878458	179	err = mthca_READ_MGM(dev, index, mailbox, &status);
1da177e4 LT	180	if (err)
	181	goto out;
	182	if (status) {
	183	mthca_err(dev, "READ_MGM returned status %02x\n", status);
	184	err = -EINVAL;
	185	goto out;
	186	}
5ceb7455	187	memset(mgm, 0, sizeof *mgm);
1da177e4	188	memcpy(mgm->gid, gid->raw, 16);
1da177e4 LT	189	}
	190
	191	for (i = 0; i < MTHCA_QP_PER_MGM; ++i)
7150bf8a JM	192	if (mgm->qp[i] == cpu_to_be32(ibqp->qp_num \| (1 << 31))) {
	193	mthca_dbg(dev, "QP %06x already a member of MGM\n",
	194	ibqp->qp_num);
	195	err = 0;
	196	goto out;
	197	} else if (!(mgm->qp[i] & cpu_to_be32(1 << 31))) {
1da177e4 LT	198	mgm->qp[i] = cpu_to_be32(ibqp->qp_num \| (1 << 31));
	199	break;
	200	}
	201
	202	if (i == MTHCA_QP_PER_MGM) {
	203	mthca_err(dev, "MGM at index %x is full.\n", index);
	204	err = -ENOMEM;
	205	goto out;
	206	}
	207
ed878458	208	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
1da177e4 LT	209	if (err)
	210	goto out;
	211	if (status) {
	212	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	213	err = -EINVAL;
5ceb7455	214	goto out;
1da177e4 LT	215	}
	216
	217	if (!link)
	218	goto out;
	219
ed878458	220	err = mthca_READ_MGM(dev, prev, mailbox, &status);
1da177e4 LT	221	if (err)
	222	goto out;
	223	if (status) {
	224	mthca_err(dev, "READ_MGM returned status %02x\n", status);
	225	err = -EINVAL;
	226	goto out;
	227	}
	228
5ceb7455	229	mgm->next_gid_index = cpu_to_be32(index << 6);
1da177e4	230
ed878458	231	err = mthca_WRITE_MGM(dev, prev, mailbox, &status);
1da177e4 LT	232	if (err)
	233	goto out;
	234	if (status) {
	235	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	236	err = -EINVAL;
	237	}
	238
	239	out:
5ceb7455 JM	240	if (err && link && index != -1) {
	241	BUG_ON(index < dev->limits.num_mgms);
	242	mthca_free(&dev->mcg_table.alloc, index);
	243	}
1da177e4	244	up(&dev->mcg_table.sem);
5ceb7455	245	err_sem:
ed878458	246	mthca_free_mailbox(dev, mailbox);
1da177e4 LT	247	return err;
	248	}
	249
	250	int mthca_multicast_detach(struct ib_qp ibqp, union ib_gid gid, u16 lid)
	251	{
	252	struct mthca_dev *dev = to_mdev(ibqp->device);
ed878458	253	struct mthca_mailbox *mailbox;
1da177e4 LT	254	struct mthca_mgm *mgm;
	255	u16 hash;
	256	int prev, index;
	257	int i, loc;
	258	int err;
	259	u8 status;
	260
ed878458 RD	261	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
	262	if (IS_ERR(mailbox))
	263	return PTR_ERR(mailbox);
	264	mgm = mailbox->buf;
1da177e4	265
5ceb7455 JM	266	if (down_interruptible(&dev->mcg_table.sem)) {
	267	err = -EINTR;
	268	goto err_sem;
	269	}
1da177e4	270
ed878458	271	err = find_mgm(dev, gid->raw, mailbox, &hash, &prev, &index);
1da177e4 LT	272	if (err)
	273	goto out;
	274
	275	if (index == -1) {
	276	mthca_err(dev, "MGID %04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x "
	277	"not found\n",
97f52eb4 SH	278	be16_to_cpu(((__be16 *) gid->raw)[0]),
	279	be16_to_cpu(((__be16 *) gid->raw)[1]),
	280	be16_to_cpu(((__be16 *) gid->raw)[2]),
	281	be16_to_cpu(((__be16 *) gid->raw)[3]),
	282	be16_to_cpu(((__be16 *) gid->raw)[4]),
	283	be16_to_cpu(((__be16 *) gid->raw)[5]),
	284	be16_to_cpu(((__be16 *) gid->raw)[6]),
	285	be16_to_cpu(((__be16 *) gid->raw)[7]));
1da177e4 LT	286	err = -EINVAL;
	287	goto out;
	288	}
	289
	290	for (loc = -1, i = 0; i < MTHCA_QP_PER_MGM; ++i) {
	291	if (mgm->qp[i] == cpu_to_be32(ibqp->qp_num \| (1 << 31)))
	292	loc = i;
	293	if (!(mgm->qp[i] & cpu_to_be32(1 << 31)))
	294	break;
	295	}
	296
	297	if (loc == -1) {
	298	mthca_err(dev, "QP %06x not found in MGM\n", ibqp->qp_num);
	299	err = -EINVAL;
	300	goto out;
	301	}
	302
	303	mgm->qp[loc] = mgm->qp[i - 1];
	304	mgm->qp[i - 1] = 0;
	305
ed878458	306	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
1da177e4 LT	307	if (err)
	308	goto out;
	309	if (status) {
	310	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	311	err = -EINVAL;
	312	goto out;
	313	}
	314
	315	if (i != 1)
	316	goto out;
	317
1da177e4 LT	318	if (prev == -1) {
1da177e4 LT	319	/* Remove entry from MGM */
5ceb7455 JM	320	int amgm_index_to_free = be32_to_cpu(mgm->next_gid_index) >> 6;
	321	if (amgm_index_to_free) {
	322	err = mthca_READ_MGM(dev, amgm_index_to_free,
ed878458	323	mailbox, &status);
1da177e4 LT	324	if (err)
	325	goto out;
	326	if (status) {
	327	mthca_err(dev, "READ_MGM returned status %02x\n",
	328	status);
	329	err = -EINVAL;
	330	goto out;
	331	}
	332	} else
	333	memset(mgm->gid, 0, 16);
	334
ed878458	335	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
1da177e4 LT	336	if (err)
	337	goto out;
	338	if (status) {
	339	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	340	err = -EINVAL;
	341	goto out;
	342	}
5ceb7455 JM	343	if (amgm_index_to_free) {
	344	BUG_ON(amgm_index_to_free < dev->limits.num_mgms);
	345	mthca_free(&dev->mcg_table.alloc, amgm_index_to_free);
	346	}
1da177e4 LT	347	} else {
1da177e4 LT	348	/* Remove entry from AMGM */
5ceb7455	349	int curr_next_index = be32_to_cpu(mgm->next_gid_index) >> 6;
ed878458	350	err = mthca_READ_MGM(dev, prev, mailbox, &status);
1da177e4 LT	351	if (err)
	352	goto out;
	353	if (status) {
	354	mthca_err(dev, "READ_MGM returned status %02x\n", status);
	355	err = -EINVAL;
	356	goto out;
	357	}
	358
5ceb7455	359	mgm->next_gid_index = cpu_to_be32(curr_next_index << 6);
1da177e4	360
ed878458	361	err = mthca_WRITE_MGM(dev, prev, mailbox, &status);
1da177e4 LT	362	if (err)
	363	goto out;
	364	if (status) {
	365	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	366	err = -EINVAL;
	367	goto out;
	368	}
5ceb7455 JM	369	BUG_ON(index < dev->limits.num_mgms);
5ceb7455 JM	370	mthca_free(&dev->mcg_table.alloc, index);
1da177e4 LT	371	}
	372
	373	out:
	374	up(&dev->mcg_table.sem);
5ceb7455	375	err_sem:
ed878458	376	mthca_free_mailbox(dev, mailbox);
1da177e4 LT	377	return err;
	378	}
	379
	380	int __devinit mthca_init_mcg_table(struct mthca_dev *dev)
	381	{
	382	int err;
5ceb7455	383	int table_size = dev->limits.num_mgms + dev->limits.num_amgms;
1da177e4 LT	384
1da177e4 LT	385	err = mthca_alloc_init(&dev->mcg_table.alloc,
5ceb7455 JM	386	table_size,
	387	table_size - 1,
	388	dev->limits.num_mgms);
1da177e4 LT	389	if (err)
	390	return err;
	391
	392	init_MUTEX(&dev->mcg_table.sem);
	393
	394	return 0;
	395	}
	396
	397	void __devexit mthca_cleanup_mcg_table(struct mthca_dev *dev)
	398	{
	399	mthca_alloc_cleanup(&dev->mcg_table.alloc);
	400	}