static void smp_new_conn(uint16_t handle, void *user_data)
{
struct test_data *data = user_data;
const struct smp_data *smp = data->test_data;
struct bthost *bthost = hciemu_client_get_host(data->hciemu);
const struct smp_req_rsp *req;
const void *pdu;
tester_print("New SMP client connection with handle 0x%04x", handle);
data->handle = handle;
bthost_add_cid_hook(bthost, handle, SMP_CID, smp_server, data);
if (smp->req_count == data->counter)
return;
req = &smp->req[data->counter];
if (!req->send)
return;
tester_print("Sending SMP PDU");
pdu = get_pdu(req->send);
bthost_send_cid(bthost, handle, SMP_CID, pdu, req->send_len);
if (!req->expect)
test_condition_complete(data);
}
static void smp_server(const void *data, uint16_t len, void *user_data)
{
struct test_data *test_data = user_data;
struct bthost *bthost = hciemu_client_get_host(test_data->hciemu);
const struct smp_data *smp = test_data->test_data;
const struct smp_req_rsp *req;
const void *pdu;
uint8_t opcode;
if (len < 1) {
tester_warn("Received too small SMP PDU");
goto failed;
}
opcode = *((const uint8_t *) data);
tester_print("Received SMP opcode 0x%02x", opcode);
if (test_data->counter >= smp->req_count) {
test_condition_complete(test_data);
return;
}
req = &smp->req[test_data->counter++];
if (!req->expect)
goto next;
if (req->expect_len != len) {
tester_warn("Unexpected SMP PDU length (%u != %u)",
len, req->expect_len);
goto failed;
}
switch (opcode) {
case 0x01: /* Pairing Request */
memcpy(test_data->preq, data, sizeof(test_data->preq));
break;
case 0x02: /* Pairing Response */
memcpy(test_data->prsp, data, sizeof(test_data->prsp));
break;
case 0x03: /* Pairing Confirm */
memcpy(test_data->pcnf, data + 1, 16);
goto next;
case 0x04: /* Pairing Random */
memcpy(test_data->rrnd, data + 1, 16);
if (!verify_random(data + 1))
goto failed;
goto next;
default:
break;
}
if (memcmp(req->expect, data, len) != 0) {
tester_warn("Unexpected SMP PDU");
goto failed;
}
next:
if (smp->req_count == test_data->counter) {
test_condition_complete(test_data);
return;
}
req = &smp->req[test_data->counter];
pdu = get_pdu(req->send);
bthost_send_cid(bthost, test_data->handle, SMP_CID, pdu,
req->send_len);
if (!req->expect)
test_condition_complete(test_data);
return;
failed:
tester_test_failed();
}
void
test_send_pdu(struct proc *p, iscsi_test_send_pdu_parameters_t *par)
{
static uint8_t pad_bytes[4] = { 0 };
test_pars_t *tp;
connection_t *conn;
pdu_t *pdu;
uint32_t psize = par->pdu_size;
void *pdu_ptr = par->pdu_ptr;
struct uio *uio;
uint32_t i, pad, dsl, size;
int s;
if ((tp = find_test_id(par->test_id)) == NULL) {
par->status = ISCSI_STATUS_INVALID_ID;
return;
}
if (!psize || pdu_ptr == NULL ||
((par->options & ISCSITEST_SFLAG_UPDATE_FIELDS) && psize < BHS_SIZE)) {
par->status = ISCSI_STATUS_PARAMETER_INVALID;
return;
}
if ((conn = tp->connection) == NULL) {
par->status = ISCSI_STATUS_TEST_INACTIVE;
return;
}
if ((pdu = get_pdu(conn, TRUE)) == NULL) {
par->status = ISCSI_STATUS_TEST_CONNECTION_CLOSED;
return;
}
DEB(1, ("Test Send PDU, id %d\n", par->test_id));
if ((par->status = map_databuf(p, &pdu_ptr, psize)) != 0) {
free_pdu(pdu);
return;
}
i = 1;
if (!par->options) {
pdu->io_vec[0].iov_base = pdu_ptr;
pdu->io_vec[0].iov_len = size = psize;
} else {
memcpy(&pdu->pdu, pdu_ptr, BHS_SIZE);
if (!(pdu->pdu.Opcode & OP_IMMEDIATE))
conn->session->CmdSN++;
pdu->pdu.p.command.CmdSN = htonl(conn->session->CmdSN);
dsl = psize - BHS_SIZE;
size = BHS_SIZE;
hton3(dsl, pdu->pdu.DataSegmentLength);
if (conn->HeaderDigest &&
!(par->options & ISCSITEST_SFLAG_NO_HEADER_DIGEST)) {
pdu->pdu.HeaderDigest = gen_digest(&pdu->pdu, BHS_SIZE);
size += 4;
}
pdu->io_vec[0].iov_base = &pdu->pdu;
pdu->io_vec[0].iov_len = size;
if (dsl) {
pdu->io_vec[1].iov_base = &pdu_ptr[BHS_SIZE];
pdu->io_vec[1].iov_len = dsl;
i++;
size += dsl;
/* Pad to next multiple of 4 */
pad = (par->options & ISCSITEST_SFLAG_NO_PADDING) ? 0 : size & 0x03;
if (pad) {
pad = 4 - pad;
pdu->io_vec[i].iov_base = pad_bytes;
pdu->io_vec[i].iov_len = pad;
i++;
size += pad;
}
if (conn->DataDigest &&
!(par->options & ISCSITEST_SFLAG_NO_DATA_DIGEST)) {
pdu->data_digest = gen_digest_2(&pdu_ptr[BHS_SIZE], dsl,
pad_bytes, pad);
pdu->io_vec[i].iov_base = &pdu->data_digest;
pdu->io_vec[i].iov_len = 4;
i++;
size += 4;
}
}
}
uio = &pdu->uio;
uio->uio_iov = pdu->io_vec;
UIO_SETUP_SYSSPACE(uio);
uio->uio_rw = UIO_WRITE;
uio->uio_iovcnt = i;
uio->uio_resid = size;
pdu->disp = PDUDISP_SIGNAL;
pdu->flags = PDUF_BUSY | PDUF_NOUPDATE;
s = splbio();
/* Enqueue for sending */
if (pdu->pdu.Opcode & OP_IMMEDIATE)
TAILQ_INSERT_HEAD(&conn->pdus_to_send, pdu, send_chain);
else
TAILQ_INSERT_TAIL(&conn->pdus_to_send, pdu, send_chain);
wakeup(&conn->pdus_to_send);
tsleep(pdu, PINOD, "test_send_pdu", 0);
splx(s);
unmap_databuf(p, pdu_ptr, psize);
par->status = ISCSI_STATUS_SUCCESS;
if (par->options & ISCSITEST_KILL_CONNECTION)
kill_connection(conn, ISCSI_STATUS_TEST_CONNECTION_CLOSED, NO_LOGOUT,
TRUE);
}