[deliverable/linux.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;

	mutex_lock(&dev->mcg_table.mutex);

	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);
	}
	mutex_unlock(&dev->mcg_table.mutex);

	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;

	mutex_lock(&dev->mcg_table.mutex);

	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:
	mutex_unlock(&dev->mcg_table.mutex);

	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;

	mutex_init(&dev->mcg_table.mutex);

	return 0;
}

void 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
fd9cfdd1	157	mutex_lock(&dev->mcg_table.mutex);
1da177e4	158
ed878458	159	err = find_mgm(dev, gid->raw, mailbox, &hash, &prev, &index);
1da177e4 LT	160	if (err)
	161	goto out;
	162
	163	if (index != -1) {
	164	if (!memcmp(mgm->gid, zero_gid, 16))
	165	memcpy(mgm->gid, gid->raw, 16);
	166	} else {
	167	link = 1;
	168
	169	index = mthca_alloc(&dev->mcg_table.alloc);
	170	if (index == -1) {
	171	mthca_err(dev, "No AMGM entries left\n");
	172	err = -ENOMEM;
	173	goto out;
	174	}
	175
ed878458	176	err = mthca_READ_MGM(dev, index, mailbox, &status);
1da177e4 LT	177	if (err)
	178	goto out;
	179	if (status) {
	180	mthca_err(dev, "READ_MGM returned status %02x\n", status);
	181	err = -EINVAL;
	182	goto out;
	183	}
5ceb7455	184	memset(mgm, 0, sizeof *mgm);
1da177e4	185	memcpy(mgm->gid, gid->raw, 16);
1da177e4 LT	186	}
	187
	188	for (i = 0; i < MTHCA_QP_PER_MGM; ++i)
7150bf8a	189	if (mgm->qp[i] == cpu_to_be32(ibqp->qp_num \| (1 << 31))) {
2fa5e2eb	190	mthca_dbg(dev, "QP %06x already a member of MGM\n",
7150bf8a JM	191	ibqp->qp_num);
	192	err = 0;
	193	goto out;
	194	} else if (!(mgm->qp[i] & cpu_to_be32(1 << 31))) {
1da177e4 LT	195	mgm->qp[i] = cpu_to_be32(ibqp->qp_num \| (1 << 31));
	196	break;
	197	}
	198
	199	if (i == MTHCA_QP_PER_MGM) {
	200	mthca_err(dev, "MGM at index %x is full.\n", index);
	201	err = -ENOMEM;
	202	goto out;
	203	}
	204
ed878458	205	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
1da177e4 LT	206	if (err)
	207	goto out;
	208	if (status) {
	209	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	210	err = -EINVAL;
5ceb7455	211	goto out;
1da177e4 LT	212	}
	213
	214	if (!link)
	215	goto out;
	216
ed878458	217	err = mthca_READ_MGM(dev, prev, mailbox, &status);
1da177e4 LT	218	if (err)
	219	goto out;
	220	if (status) {
	221	mthca_err(dev, "READ_MGM returned status %02x\n", status);
	222	err = -EINVAL;
	223	goto out;
	224	}
	225
5ceb7455	226	mgm->next_gid_index = cpu_to_be32(index << 6);
1da177e4	227
ed878458	228	err = mthca_WRITE_MGM(dev, prev, mailbox, &status);
1da177e4 LT	229	if (err)
	230	goto out;
	231	if (status) {
	232	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	233	err = -EINVAL;
	234	}
	235
	236	out:
5ceb7455 JM	237	if (err && link && index != -1) {
	238	BUG_ON(index < dev->limits.num_mgms);
	239	mthca_free(&dev->mcg_table.alloc, index);
	240	}
fd9cfdd1	241	mutex_unlock(&dev->mcg_table.mutex);
e3aa31c5	242
ed878458	243	mthca_free_mailbox(dev, mailbox);
1da177e4 LT	244	return err;
	245	}
	246
	247	int mthca_multicast_detach(struct ib_qp ibqp, union ib_gid gid, u16 lid)
	248	{
	249	struct mthca_dev *dev = to_mdev(ibqp->device);
ed878458	250	struct mthca_mailbox *mailbox;
1da177e4 LT	251	struct mthca_mgm *mgm;
	252	u16 hash;
	253	int prev, index;
	254	int i, loc;
	255	int err;
	256	u8 status;
	257
ed878458 RD	258	mailbox = mthca_alloc_mailbox(dev, GFP_KERNEL);
	259	if (IS_ERR(mailbox))
	260	return PTR_ERR(mailbox);
	261	mgm = mailbox->buf;
1da177e4	262
fd9cfdd1	263	mutex_lock(&dev->mcg_table.mutex);
1da177e4	264
ed878458	265	err = find_mgm(dev, gid->raw, mailbox, &hash, &prev, &index);
1da177e4 LT	266	if (err)
	267	goto out;
	268
	269	if (index == -1) {
	270	mthca_err(dev, "MGID %04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x "
	271	"not found\n",
97f52eb4 SH	272	be16_to_cpu(((__be16 *) gid->raw)[0]),
	273	be16_to_cpu(((__be16 *) gid->raw)[1]),
	274	be16_to_cpu(((__be16 *) gid->raw)[2]),
	275	be16_to_cpu(((__be16 *) gid->raw)[3]),
	276	be16_to_cpu(((__be16 *) gid->raw)[4]),
	277	be16_to_cpu(((__be16 *) gid->raw)[5]),
	278	be16_to_cpu(((__be16 *) gid->raw)[6]),
	279	be16_to_cpu(((__be16 *) gid->raw)[7]));
1da177e4 LT	280	err = -EINVAL;
	281	goto out;
	282	}
	283
	284	for (loc = -1, i = 0; i < MTHCA_QP_PER_MGM; ++i) {
	285	if (mgm->qp[i] == cpu_to_be32(ibqp->qp_num \| (1 << 31)))
	286	loc = i;
	287	if (!(mgm->qp[i] & cpu_to_be32(1 << 31)))
	288	break;
	289	}
	290
	291	if (loc == -1) {
	292	mthca_err(dev, "QP %06x not found in MGM\n", ibqp->qp_num);
	293	err = -EINVAL;
	294	goto out;
	295	}
	296
	297	mgm->qp[loc] = mgm->qp[i - 1];
	298	mgm->qp[i - 1] = 0;
	299
ed878458	300	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
1da177e4 LT	301	if (err)
	302	goto out;
	303	if (status) {
	304	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	305	err = -EINVAL;
	306	goto out;
	307	}
	308
	309	if (i != 1)
	310	goto out;
	311
1da177e4 LT	312	if (prev == -1) {
1da177e4 LT	313	/* Remove entry from MGM */
5ceb7455 JM	314	int amgm_index_to_free = be32_to_cpu(mgm->next_gid_index) >> 6;
	315	if (amgm_index_to_free) {
	316	err = mthca_READ_MGM(dev, amgm_index_to_free,
ed878458	317	mailbox, &status);
1da177e4 LT	318	if (err)
	319	goto out;
	320	if (status) {
	321	mthca_err(dev, "READ_MGM returned status %02x\n",
	322	status);
	323	err = -EINVAL;
	324	goto out;
	325	}
	326	} else
	327	memset(mgm->gid, 0, 16);
	328
ed878458	329	err = mthca_WRITE_MGM(dev, index, mailbox, &status);
1da177e4 LT	330	if (err)
	331	goto out;
	332	if (status) {
	333	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	334	err = -EINVAL;
	335	goto out;
	336	}
5ceb7455 JM	337	if (amgm_index_to_free) {
	338	BUG_ON(amgm_index_to_free < dev->limits.num_mgms);
	339	mthca_free(&dev->mcg_table.alloc, amgm_index_to_free);
	340	}
1da177e4 LT	341	} else {
1da177e4 LT	342	/* Remove entry from AMGM */
5ceb7455	343	int curr_next_index = be32_to_cpu(mgm->next_gid_index) >> 6;
ed878458	344	err = mthca_READ_MGM(dev, prev, mailbox, &status);
1da177e4 LT	345	if (err)
	346	goto out;
	347	if (status) {
	348	mthca_err(dev, "READ_MGM returned status %02x\n", status);
	349	err = -EINVAL;
	350	goto out;
	351	}
	352
5ceb7455	353	mgm->next_gid_index = cpu_to_be32(curr_next_index << 6);
1da177e4	354
ed878458	355	err = mthca_WRITE_MGM(dev, prev, mailbox, &status);
1da177e4 LT	356	if (err)
	357	goto out;
	358	if (status) {
	359	mthca_err(dev, "WRITE_MGM returned status %02x\n", status);
	360	err = -EINVAL;
	361	goto out;
	362	}
5ceb7455 JM	363	BUG_ON(index < dev->limits.num_mgms);
5ceb7455 JM	364	mthca_free(&dev->mcg_table.alloc, index);
1da177e4 LT	365	}
	366
	367	out:
fd9cfdd1	368	mutex_unlock(&dev->mcg_table.mutex);
e3aa31c5	369
ed878458	370	mthca_free_mailbox(dev, mailbox);
1da177e4 LT	371	return err;
	372	}
	373
	374	int __devinit mthca_init_mcg_table(struct mthca_dev *dev)
	375	{
	376	int err;
5ceb7455	377	int table_size = dev->limits.num_mgms + dev->limits.num_amgms;
1da177e4 LT	378
1da177e4 LT	379	err = mthca_alloc_init(&dev->mcg_table.alloc,
5ceb7455 JM	380	table_size,
	381	table_size - 1,
	382	dev->limits.num_mgms);
1da177e4 LT	383	if (err)
	384	return err;
	385
fd9cfdd1	386	mutex_init(&dev->mcg_table.mutex);
1da177e4 LT	387
	388	return 0;
	389	}
	390
e1f7868c	391	void mthca_cleanup_mcg_table(struct mthca_dev *dev)
1da177e4 LT	392	{
	393	mthca_alloc_cleanup(&dev->mcg_table.alloc);
	394	}