自学教程：C++ DBGC2函数代码示例

/** * Parse OCSP responder ID * * @v ocsp		OCSP check * @v raw		ASN.1 cursor * @ret rc		Return status code */static int ocsp_parse_responder_id ( struct ocsp_check *ocsp,				     const struct asn1_cursor *raw ) {	struct ocsp_responder *responder = &ocsp->response.responder;	struct asn1_cursor *responder_id = &responder->id;	unsigned int type;	/* Enter responder ID */	memcpy ( responder_id, raw, sizeof ( *responder_id ) );	type = asn1_type ( responder_id );	asn1_enter_any ( responder_id );	/* Identify responder ID type */	switch ( type ) {	case ASN1_EXPLICIT_TAG ( 1 ) :		DBGC2 ( ocsp, "OCSP %p /"%s/" responder identified by name/n",			ocsp, x509_name ( ocsp->cert ) );		responder->compare = ocsp_compare_responder_name;		return 0;	case ASN1_EXPLICIT_TAG ( 2 ) :		DBGC2 ( ocsp, "OCSP %p /"%s/" responder identified by key "			"hash/n", ocsp, x509_name ( ocsp->cert ) );		responder->compare = ocsp_compare_responder_key_hash;		return 0;	default:		DBGC ( ocsp, "OCSP %p /"%s/" unsupported responder ID type "		       "%d/n", ocsp, x509_name ( ocsp->cert ), type );		return -ENOTSUP_RESPONDER_ID;	}}

/** * Poll for completed packets * * @v netdev		Network device */static void netfront_poll_tx ( struct net_device *netdev ) {	struct netfront_nic *netfront = netdev->priv;	struct xen_device *xendev = netfront->xendev;	struct netif_tx_response *response;	struct io_buffer *iobuf;	unsigned int status;	int rc;	/* Consume any unconsumed responses */	while ( RING_HAS_UNCONSUMED_RESPONSES ( &netfront->tx_fring ) ) {		/* Get next response */		response = RING_GET_RESPONSE ( &netfront->tx_fring,					       netfront->tx_fring.rsp_cons++ );		/* Retrieve from descriptor ring */		iobuf = netfront_pull ( netfront, &netfront->tx, response->id );		status = response->status;		if ( status == NETIF_RSP_OKAY ) {			DBGC2 ( netfront, "NETFRONT %s TX id %d complete/n",				xendev->key, response->id );			netdev_tx_complete ( netdev, iobuf );		} else {			rc = -EIO_NETIF_RSP ( status );			DBGC2 ( netfront, "NETFRONT %s TX id %d error %d: %s/n",				xendev->key, response->id, status,				strerror ( rc ) );			netdev_tx_complete_err ( netdev, iobuf, rc );		}	}}

/** * Receive XenStore response * * @v xen		Xen hypervisor * @v req_id		Request ID * @v value		Value to fill in * @v len		Length to fill in * @ret rc		Return status code * * The caller is responsible for eventually calling free() on the * returned value.  Note that the value may comprise multiple * NUL-terminated strings concatenated together.  A terminating NUL * will always be appended to the returned value. */static int xenstore_response ( struct xen_hypervisor *xen, uint32_t req_id,			       char **value, size_t *len ) {	struct xsd_sockmsg msg;	char *string;	int rc;	/* Receive message header */	xenstore_recv ( xen, &msg, sizeof ( msg ) );	*len = msg.len;	/* Allocate space for response */	*value = zalloc ( msg.len + 1 /* terminating NUL */ );	/* Receive data.  Do this even if allocation failed, or if the	 * request ID was incorrect, to avoid leaving data in the	 * ring.	 */	xenstore_recv ( xen, *value, msg.len );	/* Validate request ID */	if ( msg.req_id != req_id ) {		DBGC ( xen, "XENSTORE response ID mismatch (got %d, expected "		       "%d)/n", msg.req_id, req_id );		rc = -EPROTO;		goto err_req_id;	}	/* Check for allocation failure */	if ( ! *value ) {		DBGC ( xen, "XENSTORE could not allocate %d bytes for "		       "response/n", msg.len );		rc = -ENOMEM;		goto err_alloc;	}	/* Check for explicit errors */	if ( msg.type == XS_ERROR ) {		DBGC ( xen, "XENSTORE response error /"%s/"/n", *value );		rc = -EIO;		goto err_explicit;	}	DBGC2 ( xen, "XENSTORE response ID %d/n", req_id );	if ( DBG_EXTRA ) {		for ( string = *value ; string < ( *value + msg.len ) ;		      string += ( strlen ( string ) + 1 /* NUL */ ) ) {			DBGC2 ( xen, " - /"%s/"/n", string );		}	}	return 0; err_explicit: err_alloc: err_req_id:	free ( *value );	*value = NULL;	return rc;}

/** * Generate secure pseudo-random data using a single hash function * * @v tls		TLS session * @v digest		Hash function to use * @v secret		Secret * @v secret_len	Length of secret * @v out		Output buffer * @v out_len		Length of output buffer * @v seeds		( data, len ) pairs of seed data, terminated by NULL */static void tls_p_hash_va ( struct tls_session *tls,			    struct digest_algorithm *digest,			    void *secret, size_t secret_len,			    void *out, size_t out_len,			    va_list seeds ) {	uint8_t secret_copy[secret_len];	uint8_t digest_ctx[digest->ctxsize];	uint8_t digest_ctx_partial[digest->ctxsize];	uint8_t a[digest->digestsize];	uint8_t out_tmp[digest->digestsize];	size_t frag_len = digest->digestsize;	va_list tmp;	/* Copy the secret, in case HMAC modifies it */	memcpy ( secret_copy, secret, secret_len );	secret = secret_copy;	DBGC2 ( tls, "TLS %p %s secret:/n", tls, digest->name );	DBGC2_HD ( tls, secret, secret_len );	/* Calculate A(1) */	hmac_init ( digest, digest_ctx, secret, &secret_len );	va_copy ( tmp, seeds );	tls_hmac_update_va ( digest, digest_ctx, tmp );	va_end ( tmp );	hmac_final ( digest, digest_ctx, secret, &secret_len, a );	DBGC2 ( tls, "TLS %p %s A(1):/n", tls, digest->name );	DBGC2_HD ( tls, &a, sizeof ( a ) );	/* Generate as much data as required */	while ( out_len ) {		/* Calculate output portion */		hmac_init ( digest, digest_ctx, secret, &secret_len );		hmac_update ( digest, digest_ctx, a, sizeof ( a ) );		memcpy ( digest_ctx_partial, digest_ctx, digest->ctxsize );		va_copy ( tmp, seeds );		tls_hmac_update_va ( digest, digest_ctx, tmp );		va_end ( tmp );		hmac_final ( digest, digest_ctx,			     secret, &secret_len, out_tmp );		/* Copy output */		if ( frag_len > out_len )			frag_len = out_len;		memcpy ( out, out_tmp, frag_len );		DBGC2 ( tls, "TLS %p %s output:/n", tls, digest->name );		DBGC2_HD ( tls, out, frag_len );		/* Calculate A(i) */		hmac_final ( digest, digest_ctx_partial,			     secret, &secret_len, a );		DBGC2 ( tls, "TLS %p %s A(n):/n", tls, digest->name );		DBGC2_HD ( tls, &a, sizeof ( a ) );		out += frag_len;		out_len -= frag_len;	}}

/** * Send XenStore request * * @v xen		Xen hypervisor * @v type		Message type * @v req_id		Request ID * @v value		Value, or NULL to omit * @v key		Key path components * @ret rc		Return status code */static int xenstore_request ( struct xen_hypervisor *xen,			      enum xsd_sockmsg_type type, uint32_t req_id,			      const char *value, va_list key ) {	struct xsd_sockmsg msg;	struct evtchn_send event;	const char *string;	va_list tmp;	int xenrc;	int rc;	/* Construct message header */	msg.type = type;	msg.req_id = req_id;	msg.tx_id = 0;	msg.len = 0;	DBGC2 ( xen, "XENSTORE request ID %d type %d ", req_id, type );	/* Calculate total length */	va_copy ( tmp, key );	while ( ( string = va_arg ( tmp, const char * ) ) != NULL ) {		DBGC2 ( xen, "%s%s", ( msg.len ? "/" : "" ), string );		msg.len += ( strlen ( string ) + 1 /* '/' or NUL */ );	}	va_end ( tmp );	if ( value ) {		DBGC2 ( xen, " = /"%s/"", value );		msg.len += strlen ( value );	}	DBGC2 ( xen, "/n" );	/* Send message */	xenstore_send ( xen, &msg, sizeof ( msg ) );	string = va_arg ( key, const char * );	assert ( string != NULL );	xenstore_send_string ( xen, string );	while ( ( string = va_arg ( key, const char * ) ) != NULL ) {		xenstore_send_string ( xen, "/" );		xenstore_send_string ( xen, string );	}	xenstore_send ( xen, "", 1 ); /* Separating NUL */	if ( value )		xenstore_send_string ( xen, value );	/* Notify the back end */	event.port = xen->store.port;	if ( ( xenrc = xenevent_send ( xen, &event ) ) != 0 ) {		rc = -EXEN ( xenrc );		DBGC ( xen, "XENSTORE could not notify back end: %s/n",		       strerror ( rc ) );		return rc;	}	return 0;}

/** * Parse OCSP certificates * * @v ocsp		OCSP check * @v raw		ASN.1 cursor * @ret rc		Return status code */static int ocsp_parse_certs ( struct ocsp_check *ocsp,			      const struct asn1_cursor *raw ) {	struct ocsp_response *response = &ocsp->response;	struct asn1_cursor cursor;	struct x509_certificate *cert;	int rc;	/* Enter certs */	memcpy ( &cursor, raw, sizeof ( cursor ) );	asn1_enter ( &cursor, ASN1_EXPLICIT_TAG ( 0 ) );	asn1_enter ( &cursor, ASN1_SEQUENCE );	/* Parse certificate, if present.  The data structure permits	 * multiple certificates, but the protocol requires that the	 * OCSP signing certificate must either be the issuer itself,	 * or must be directly issued by the issuer (see RFC2560	 * section 4.2.2.2 "Authorized Responders").  We therefore	 * need to identify only the single certificate matching the	 * Responder ID.	 */	while ( cursor.len ) {		/* Parse certificate */		if ( ( rc = x509_certificate ( cursor.data, cursor.len,					       &cert ) ) != 0 ) {			DBGC ( ocsp, "OCSP %p /"%s/" could not parse "			       "certificate: %s/n", ocsp,			       x509_name ( ocsp->cert ), strerror ( rc ) );			DBGC_HDA ( ocsp, 0, cursor.data, cursor.len );			return rc;		}		/* Use if this certificate matches the responder ID */		if ( response->responder.compare ( ocsp, cert ) == 0 ) {			response->signer = cert;			DBGC2 ( ocsp, "OCSP %p /"%s/" response is signed by ",				ocsp, x509_name ( ocsp->cert ) );			DBGC2 ( ocsp, "/"%s/"/n",				x509_name ( response->signer ) );			return 0;		}		/* Otherwise, discard this certificate */		x509_put ( cert );		asn1_skip_any ( &cursor );	}	DBGC ( ocsp, "OCSP %p /"%s/" missing responder certificate/n",	       ocsp, x509_name ( ocsp->cert ) );	return -EACCES_NO_RESPONDER;}

/** * Issue command with parameter block and data block * * @v nii		NII NIC * @v op		Operation * @v cpb		Command parameter block, or NULL * @v cpb_len		Command parameter block length * @v db		Data block, or NULL * @v db_len		Data block length * @ret stat		Status flags, or negative status code */static int nii_issue_cpb_db ( struct nii_nic *nii, unsigned int op, void *cpb,			      size_t cpb_len, void *db, size_t db_len ) {	PXE_CDB cdb;	/* Prepare command descriptor block */	memset ( &cdb, 0, sizeof ( cdb ) );	cdb.OpCode = NII_OPCODE ( op );	cdb.OpFlags = NII_OPFLAGS ( op );	cdb.CPBaddr = ( ( intptr_t ) cpb );	cdb.CPBsize = cpb_len;	cdb.DBaddr = ( ( intptr_t ) db );	cdb.DBsize = db_len;	cdb.IFnum = nii->nii->IfNum;	/* Issue command */	DBGC2 ( nii, "NII %s issuing %02x:%04x ifnum %d%s%s/n",		nii->dev.name, cdb.OpCode, cdb.OpFlags, cdb.IFnum,		( cpb ? " cpb" : "" ), ( db ? " db" : "" ) );	if ( cpb )		DBGC2_HD ( nii, cpb, cpb_len );	if ( db )		DBGC2_HD ( nii, db, db_len );	nii->issue ( ( intptr_t ) &cdb );	/* Check completion status */	if ( cdb.StatCode != PXE_STATCODE_SUCCESS )		return -cdb.StatCode;	/* Return command-specific status flags */	return ( cdb.StatFlags & ~PXE_STATFLAGS_STATUS_MASK );}

/** * Fetch MAC address * * @v netfront		Netfront device * @v hw_addr		Hardware address to fill in * @ret rc		Return status code */static int netfront_read_mac ( struct netfront_nic *netfront, void *hw_addr ) {	struct xen_device *xendev = netfront->xendev;	struct xen_hypervisor *xen = xendev->xen;	char *mac;	int len;	int rc;	/* Fetch MAC address */	if ( ( rc = xenstore_read ( xen, &mac, xendev->key, "mac", NULL ) )!=0){		DBGC ( netfront, "NETFRONT %s could not read MAC address: %s/n",		       xendev->key, strerror ( rc ) );		goto err_xenstore_read;	}	DBGC2 ( netfront, "NETFRONT %s has MAC address /"%s/"/n",		xendev->key, mac );	/* Decode MAC address */	len = hex_decode ( mac, ':', hw_addr, ETH_ALEN );	if ( len < 0 ) {		rc = len;		DBGC ( netfront, "NETFRONT %s could not decode MAC address "		       "/"%s/": %s/n", xendev->key, mac, strerror ( rc ) );		goto err_decode;	}	/* Success */	rc = 0; err_decode:	free ( mac ); err_xenstore_read:	return rc;}

/** * Write to MII register * * @v mii		MII interface * @v reg		Register address * @v data		Data to write * @ret rc		Return status code */static int rhine_mii_write ( struct mii_interface *mii, unsigned int reg,                             unsigned int data ) {	struct rhine_nic *rhn = container_of ( mii, struct rhine_nic, mii );	unsigned int timeout = RHINE_TIMEOUT_US;	uint8_t cr;	DBGC2 ( rhn, "RHINE %p MII write reg %d data 0x%04x/n",	        rhn, reg, data );	/* Initiate write */	writeb ( reg, rhn->regs + RHINE_MII_ADDR );	writew ( data, rhn->regs + RHINE_MII_RDWR );	cr = readb ( rhn->regs + RHINE_MII_CR );	writeb ( ( cr | RHINE_MII_CR_WREN ), rhn->regs + RHINE_MII_CR );	/* Wait for write to complete */	while ( timeout-- ) {		udelay ( 1 );		cr = readb ( rhn->regs + RHINE_MII_CR );		if ( ! ( cr & RHINE_MII_CR_WREN ) )			return 0;	}	DBGC ( rhn, "RHINE %p MII write timeout/n", rhn );	return -ETIMEDOUT;}

/** * Poll for completed packets * * @v netdev		Network device */static void rhine_poll_tx ( struct net_device *netdev ) {	struct rhine_nic *rhn = netdev->priv;	struct rhine_descriptor *desc;	unsigned int tx_idx;	uint32_t des0;	/* Check for completed packets */	while ( rhn->tx.cons != rhn->tx.prod ) {		/* Get next transmit descriptor */		tx_idx = ( rhn->tx.cons % RHINE_TXDESC_NUM );		desc = &rhn->tx.desc[tx_idx];		/* Stop if descriptor is still in use */		if ( desc->des0 & cpu_to_le32 ( RHINE_DES0_OWN ) )			return;		/* Complete TX descriptor */		des0 = le32_to_cpu ( desc->des0 );		if ( des0 & RHINE_TDES0_TERR ) {			DBGC ( rhn, "RHINE %p TX %d error (DES0 %08x)/n",			       rhn, tx_idx, des0 );			netdev_tx_complete_next_err ( netdev, -EIO );		} else {			DBGC2 ( rhn, "RHINE %p TX %d complete/n", rhn, tx_idx );			netdev_tx_complete_next ( netdev );		}		rhn->tx.cons++;	}}

/** * Transmit packet * * @v netdev		Network device * @v iobuf		I/O buffer * @ret rc		Return status code */static int rhine_transmit ( struct net_device *netdev,                            struct io_buffer *iobuf ) {	struct rhine_nic *rhn = netdev->priv;	struct rhine_descriptor *desc;	physaddr_t address;	unsigned int tx_idx;	/* Get next transmit descriptor */	if ( ( rhn->tx.prod - rhn->tx.cons ) >= RHINE_TXDESC_NUM )		return -ENOBUFS;	tx_idx = ( rhn->tx.prod++ % RHINE_TXDESC_NUM );	desc = &rhn->tx.desc[tx_idx];	/* Pad and align packet */	iob_pad ( iobuf, ETH_ZLEN );	address = virt_to_bus ( iobuf->data );	/* Populate transmit descriptor */	desc->buffer = cpu_to_le32 ( address );	desc->des1 = cpu_to_le32 ( RHINE_DES1_IC | RHINE_TDES1_STP |				   RHINE_TDES1_EDP | RHINE_DES1_CHAIN |				   RHINE_DES1_SIZE ( iob_len ( iobuf ) ) );	wmb();	desc->des0 = cpu_to_le32 ( RHINE_DES0_OWN );	wmb();	/* Notify card that there are packets ready to transmit */	writeb ( ( rhn->cr1 | RHINE_CR1_TXPOLL ), rhn->regs + RHINE_CR1 );	DBGC2 ( rhn, "RHINE %p TX %d is [%llx,%llx)/n", rhn, tx_idx,		( ( unsigned long long ) address ),		( ( unsigned long long ) address + iob_len ( iobuf ) ) );	return 0;}

/** * Poll for completed packets * * @v netdev		Network device */static void myson_poll_tx ( struct net_device *netdev ) {	struct myson_nic *myson = netdev->priv;	struct myson_descriptor *tx;	unsigned int tx_idx;	/* Check for completed packets */	while ( myson->tx.cons != myson->tx.prod ) {		/* Get next transmit descriptor */		tx_idx = ( myson->tx.cons % MYSON_NUM_TX_DESC );		tx = &myson->tx.desc[tx_idx];		/* Stop if descriptor is still in use */		if ( tx->status & cpu_to_le32 ( MYSON_TX_STAT_OWN ) )			return;		/* Complete TX descriptor */		if ( tx->status & cpu_to_le32 ( MYSON_TX_STAT_ABORT |						MYSON_TX_STAT_CSL ) ) {			DBGC ( myson, "MYSON %p TX %d completion error "			       "(%08x)/n", myson, tx_idx,			       le32_to_cpu ( tx->status ) );			netdev_tx_complete_next_err ( netdev, -EIO );		} else {			DBGC2 ( myson, "MYSON %p TX %d complete/n",				myson, tx_idx );			netdev_tx_complete_next ( netdev );		}		myson->tx.cons++;	}}

/** * Encapsulate and encrypt a packet using CCMP * * @v crypto	CCMP cryptosystem * @v iob	I/O buffer containing cleartext packet * @ret eiob	I/O buffer containing encrypted packet */struct io_buffer * ccmp_encrypt ( struct net80211_crypto *crypto,				  struct io_buffer *iob ){	struct ccmp_ctx *ctx = crypto->priv;	struct ieee80211_frame *hdr = iob->data;	struct io_buffer *eiob;	const int hdrlen = IEEE80211_TYP_FRAME_HEADER_LEN;	int datalen = iob_len ( iob ) - hdrlen;	struct ccmp_head head;	struct ccmp_nonce nonce;	struct ccmp_aad aad;	u8 mic[8], tx_pn[6];	void *edata, *emic;	ctx->tx_seq++;	u64_to_pn ( ctx->tx_seq, tx_pn, PN_LSB );	/* Allocate memory */	eiob = alloc_iob ( iob_len ( iob ) + CCMP_HEAD_LEN + CCMP_MIC_LEN );	if ( ! eiob )		return NULL;	/* Copy frame header */	memcpy ( iob_put ( eiob, hdrlen ), iob->data, hdrlen );	hdr = eiob->data;	hdr->fc |= IEEE80211_FC_PROTECTED;	/* Fill in packet number and extended IV */	memcpy ( head.pn_lo, tx_pn, 2 );	memcpy ( head.pn_hi, tx_pn + 2, 4 );	head.kid = 0x20;	/* have Extended IV, key ID 0 */	head._rsvd = 0;	memcpy ( iob_put ( eiob, sizeof ( head ) ), &head, sizeof ( head ) );	/* Form nonce */	nonce.prio = 0;	memcpy ( nonce.a2, hdr->addr2, ETH_ALEN );	u64_to_pn ( ctx->tx_seq, nonce.pn, PN_MSB );	/* Form additional authentication data */	aad.fc = hdr->fc & CCMP_AAD_FC_MASK;	memcpy ( aad.a1, hdr->addr1, 3 * ETH_ALEN ); /* all 3 at once */	aad.seq = hdr->seq & CCMP_AAD_SEQ_MASK;	/* Calculate MIC over the data */	ccmp_cbc_mac ( ctx, &nonce, iob->data + hdrlen, datalen, &aad, mic );	/* Copy and encrypt data and MIC */	edata = iob_put ( eiob, datalen );	emic = iob_put ( eiob, CCMP_MIC_LEN );	ccmp_ctr_xor ( ctx, &nonce,		       iob->data + hdrlen, edata, datalen,		       mic, emic );	/* Done! */	DBGC2 ( ctx, "WPA-CCMP %p: encrypted packet %p -> %p/n", ctx,		iob, eiob );	return eiob;}

/** * Handle SRP login response * * @v srp		SRP device * @v iobuf		I/O buffer * @ret rc		Return status code */static int srp_login_rsp ( struct srp_device *srp, struct io_buffer *iobuf ) {	struct srp_login_rsp *login_rsp = iobuf->data;	int rc;	DBGC2 ( srp, "SRP %p RX login response tag %08x%08x/n",		srp, ntohl ( login_rsp->tag.dwords[0] ),		ntohl ( login_rsp->tag.dwords[1] ) );	/* Sanity check */	if ( iob_len ( iobuf ) < sizeof ( *login_rsp ) ) {		DBGC ( srp, "SRP %p RX login response too short (%zd bytes)/n",		       srp, iob_len ( iobuf ) );		rc = -EINVAL;		goto out;	}	DBGC ( srp, "SRP %p logged in/n", srp );	/* Mark as logged in */	srp->state |= SRP_STATE_LOGGED_IN;	/* Reset error counter */	srp->retry_count = 0;	/* Issue pending command */	srp_cmd ( srp );	rc = 0; out:	free_iob ( iobuf );	return rc;}

/** * Handle SRP login rejection * * @v srp		SRP device * @v iobuf		I/O buffer * @ret rc		Return status code */static int srp_login_rej ( struct srp_device *srp, struct io_buffer *iobuf ) {	struct srp_login_rej *login_rej = iobuf->data;	int rc;	DBGC2 ( srp, "SRP %p RX login rejection tag %08x%08x/n",		srp, ntohl ( login_rej->tag.dwords[0] ),		ntohl ( login_rej->tag.dwords[1] ) );	/* Sanity check */	if ( iob_len ( iobuf ) < sizeof ( *login_rej ) ) {		DBGC ( srp, "SRP %p RX login rejection too short (%zd "		       "bytes)/n", srp, iob_len ( iobuf ) );		rc = -EINVAL;		goto out;	}	/* Login rejection always indicates an error */	DBGC ( srp, "SRP %p login rejected (reason %08x)/n",	       srp, ntohl ( login_rej->reason ) );	rc = -EPERM; out:	free_iob ( iobuf );	return rc;}

/** * Reset the network device * * @v snp		SNP interface * @v ext_verify	Extended verification required * @ret efirc		EFI status code */static EFI_STATUS EFIAPIefi_snp_reset ( EFI_SIMPLE_NETWORK_PROTOCOL *snp, BOOLEAN ext_verify ) {	struct efi_snp_device *snpdev =		container_of ( snp, struct efi_snp_device, snp );	int rc;	DBGC2 ( snpdev, "SNPDEV %p RESET (%s extended verification)/n",		snpdev, ( ext_verify ? "with" : "without" ) );	/* Fail if net device is currently claimed for use by iPXE */	if ( efi_snp_claimed )		return EFI_NOT_READY;	netdev_close ( snpdev->netdev );	efi_snp_set_state ( snpdev );	if ( ( rc = netdev_open ( snpdev->netdev ) ) != 0 ) {		DBGC ( snpdev, "SNPDEV %p could not reopen %s: %s/n",		       snpdev, snpdev->netdev->name, strerror ( rc ) );		return EFIRC ( rc );	}	efi_snp_set_state ( snpdev );	return 0;}

/** * Transmit SRP SCSI command * * @v srp		SRP device */static void srp_cmd ( struct srp_device *srp ) {	struct io_buffer *iobuf;	struct srp_cmd *cmd;	struct srp_memory_descriptor *data_out;	struct srp_memory_descriptor *data_in;	int rc;	assert ( srp->state & SRP_STATE_LOGGED_IN );	/* Allocate I/O buffer */	iobuf = xfer_alloc_iob ( &srp->socket, SRP_MAX_I_T_IU_LEN );	if ( ! iobuf ) {		rc = -ENOMEM;		goto err;	}	/* Construct base portion */	cmd = iob_put ( iobuf, sizeof ( *cmd ) );	memset ( cmd, 0, sizeof ( *cmd ) );	cmd->type = SRP_CMD;	cmd->tag.dwords[1] = htonl ( ++srp_tag );	cmd->lun = srp->lun;	memcpy ( &cmd->cdb, &srp->command->cdb, sizeof ( cmd->cdb ) );	/* Construct data-out descriptor, if present */	if ( srp->command->data_out ) {		cmd->data_buffer_formats |= SRP_CMD_DO_FMT_DIRECT;		data_out = iob_put ( iobuf, sizeof ( *data_out ) );		data_out->address =		    cpu_to_be64 ( user_to_phys ( srp->command->data_out, 0 ) );		data_out->handle = ntohl ( srp->memory_handle );		data_out->len = ntohl ( srp->command->data_out_len );	}	/* Construct data-in descriptor, if present */	if ( srp->command->data_in ) {		cmd->data_buffer_formats |= SRP_CMD_DI_FMT_DIRECT;		data_in = iob_put ( iobuf, sizeof ( *data_in ) );		data_in->address =		     cpu_to_be64 ( user_to_phys ( srp->command->data_in, 0 ) );		data_in->handle = ntohl ( srp->memory_handle );		data_in->len = ntohl ( srp->command->data_in_len );	}	DBGC2 ( srp, "SRP %p TX SCSI command tag %08x%08x/n", srp,		ntohl ( cmd->tag.dwords[0] ), ntohl ( cmd->tag.dwords[1] ) );	DBGC2_HDA ( srp, 0, iobuf->data, iob_len ( iobuf ) );	/* Send IU */	if ( ( rc = xfer_deliver_iob ( &srp->socket, iobuf ) ) != 0 ) {		DBGC ( srp, "SRP %p could not send command: %s/n",		       srp, strerror ( rc ) );		goto err;	}	return; err:	srp_fail ( srp, rc );}

/** * Refill RX descriptor ring * * @v rhn		Rhine device */static void rhine_refill_rx ( struct rhine_nic *rhn ) {	struct rhine_descriptor *desc;	struct io_buffer *iobuf;	unsigned int rx_idx;	physaddr_t address;	while ( ( rhn->rx.prod - rhn->rx.cons ) < RHINE_RXDESC_NUM ) {		/* Allocate I/O buffer */		iobuf = alloc_iob ( RHINE_RX_MAX_LEN );		if ( ! iobuf ) {			/* Wait for next refill */			return;		}		/* Populate next receive descriptor */		rx_idx = ( rhn->rx.prod++ % RHINE_RXDESC_NUM );		desc = &rhn->rx.desc[rx_idx];		address = virt_to_bus ( iobuf->data );		desc->buffer = cpu_to_le32 ( address );		desc->des1 =			cpu_to_le32 ( RHINE_DES1_SIZE ( RHINE_RX_MAX_LEN - 1) |				      RHINE_DES1_CHAIN | RHINE_DES1_IC );		wmb();		desc->des0 = cpu_to_le32 ( RHINE_DES0_OWN );		/* Record I/O buffer */		rhn->rx_iobuf[rx_idx] = iobuf;		DBGC2 ( rhn, "RHINE %p RX %d is [%llx,%llx)/n", rhn, rx_idx,			( ( unsigned long long ) address ),			( ( unsigned long long ) address + RHINE_RX_MAX_LEN ) );	}}

/** * Transmit an ethernet packet. * * The packet can be written to the socket and marked as complete immediately. */static int af_packet_nic_transmit ( struct net_device *netdev,				    struct io_buffer *iobuf ){	struct af_packet_nic * nic = netdev->priv;	struct sockaddr_ll socket_address;	const struct ethhdr * eh;	int rc;	memset(&socket_address, 0, sizeof(socket_address));	socket_address.sll_family = LINUX_AF_PACKET;	socket_address.sll_ifindex = nic->ifindex;	socket_address.sll_halen = ETH_ALEN;	eh = iobuf->data;	memcpy(socket_address.sll_addr, eh->h_dest, ETH_ALEN);	rc = linux_sendto(nic->fd, iobuf->data, iobuf->tail - iobuf->data,			  0, (struct sockaddr *)&socket_address,			  sizeof(socket_address));	DBGC2(nic, "af_packet %p wrote %d bytes/n", nic, rc);	netdev_tx_complete(netdev, iobuf);	return 0;}

/** * Build OCSP request * * @v ocsp		OCSP check * @ret rc		Return status code */static int ocsp_request ( struct ocsp_check *ocsp ) {	struct digest_algorithm *digest = &ocsp_digest_algorithm;	struct asn1_builder *builder = &ocsp->request.builder;	struct asn1_cursor *cert_id_tail = &ocsp->request.cert_id_tail;	uint8_t digest_ctx[digest->ctxsize];	uint8_t name_digest[digest->digestsize];	uint8_t pubkey_digest[digest->digestsize];	int rc;	/* Generate digests */	digest_init ( digest, digest_ctx );	digest_update ( digest, digest_ctx, ocsp->cert->issuer.raw.data,			ocsp->cert->issuer.raw.len );	digest_final ( digest, digest_ctx, name_digest );	digest_init ( digest, digest_ctx );	digest_update ( digest, digest_ctx,			ocsp->issuer->subject.public_key.raw_bits.data,			ocsp->issuer->subject.public_key.raw_bits.len );	digest_final ( digest, digest_ctx, pubkey_digest );	/* Construct request */	if ( ( rc = ( asn1_prepend_raw ( builder, ocsp->cert->serial.raw.data,					 ocsp->cert->serial.raw.len ),		      asn1_prepend ( builder, ASN1_OCTET_STRING,				     pubkey_digest, sizeof ( pubkey_digest ) ),		      asn1_prepend ( builder, ASN1_OCTET_STRING,				     name_digest, sizeof ( name_digest ) ),		      asn1_prepend ( builder, ASN1_SEQUENCE,				     ocsp_algorithm_id,				     sizeof ( ocsp_algorithm_id ) ),		      asn1_wrap ( builder, ASN1_SEQUENCE ),		      asn1_wrap ( builder, ASN1_SEQUENCE ),		      asn1_wrap ( builder, ASN1_SEQUENCE ),		      asn1_wrap ( builder, ASN1_SEQUENCE ),		      asn1_wrap ( builder, ASN1_SEQUENCE ) ) ) != 0 ) {		DBGC ( ocsp, "OCSP %p /"%s/" could not build request: %s/n",		       ocsp, x509_name ( ocsp->cert ), strerror ( rc ) );		return rc;	}	DBGC2 ( ocsp, "OCSP %p /"%s/" request is:/n",		ocsp, x509_name ( ocsp->cert ) );	DBGC2_HDA ( ocsp, 0, builder->data, builder->len );	/* Parse certificate ID for comparison with response */	cert_id_tail->data = builder->data;	cert_id_tail->len = builder->len;	if ( ( rc = ( asn1_enter ( cert_id_tail, ASN1_SEQUENCE ),		      asn1_enter ( cert_id_tail, ASN1_SEQUENCE ),		      asn1_enter ( cert_id_tail, ASN1_SEQUENCE ),		      asn1_enter ( cert_id_tail, ASN1_SEQUENCE ),		      asn1_enter ( cert_id_tail, ASN1_SEQUENCE ),		      asn1_skip ( cert_id_tail, ASN1_SEQUENCE ) ) ) != 0 ) {		DBGC ( ocsp, "OCSP %p /"%s/" could not locate certID: %s/n",		       ocsp, x509_name ( ocsp->cert ), strerror ( rc ) );		return rc;	}	return 0;}

/** * Build OCSP URI string * * @v ocsp		OCSP check * @ret rc		Return status code */static int ocsp_uri_string ( struct ocsp_check *ocsp ) {	struct x509_ocsp_responder *responder =		&ocsp->cert->extensions.auth_info.ocsp;	char *base64;	char *sep;	size_t base64_len;	size_t uri_len;	size_t len;	int rc;	/* Sanity check */	if ( ! responder->uri.len ) {		DBGC ( ocsp, "OCSP %p /"%s/" has no OCSP URI/n",		       ocsp, x509_name ( ocsp->cert ) );		rc = -ENOTTY;		goto err_no_uri;	}	/* Calculate base64-encoded request length */	base64_len = ( base64_encoded_len ( ocsp->request.builder.len )		       + 1 /* NUL */ );	/* Allocate and construct the base64-encoded request */	base64 = malloc ( base64_len );	if ( ! base64 ) {		rc = -ENOMEM;		goto err_alloc_base64;	}	base64_encode ( ocsp->request.builder.data, ocsp->request.builder.len,			base64, base64_len );	/* Calculate URI-encoded base64-encoded request length */	uri_len = ( uri_encode ( URI_PATH, base64, ( base64_len - 1 /* NUL */ ),				 NULL, 0 ) + 1 /* NUL */ );	/* Allocate and construct the URI string */	len = ( responder->uri.len + 1 /* possible "/" */ + uri_len );	ocsp->uri_string = zalloc ( len );	if ( ! ocsp->uri_string ) {		rc = -ENOMEM;		goto err_alloc_uri;	}	memcpy ( ocsp->uri_string, responder->uri.data, responder->uri.len );	sep = &ocsp->uri_string[ responder->uri.len - 1 ];	if ( *sep != '/' )		*(++sep) = '/';	uri_encode ( URI_PATH, base64, base64_len, ( sep + 1 ), uri_len );	DBGC2 ( ocsp, "OCSP %p /"%s/" URI is %s/n",		ocsp, x509_name ( ocsp->cert ), ocsp->uri_string );	/* Success */	rc = 0; err_alloc_uri:	free ( base64 ); err_alloc_base64: err_no_uri:	return rc;}

/** * Allocate grant references * * @v xen		Xen hypervisor * @v refs		Grant references to fill in * @v count		Number of references * @ret rc		Return status code */int xengrant_alloc ( struct xen_hypervisor *xen, grant_ref_t *refs,		     unsigned int count ) {	struct grant_entry_header *hdr;	unsigned int entries = xengrant_entries ( xen );	unsigned int mask = ( entries - 1 );	unsigned int check = 0;	unsigned int avail;	unsigned int ref;	/* Fail unless we have enough references available */	avail = ( entries - xen->grant.used - GNTTAB_NR_RESERVED_ENTRIES );	if ( avail < count ) {		DBGC ( xen, "XENGRANT cannot allocate %d references (only %d "		       "of %d available)/n", count, avail, entries );		return -ENOBUFS;	}	DBGC ( xen, "XENGRANT allocating %d references (from %d of %d "	       "available)/n", count, avail, entries );	/* Update number of references used */	xen->grant.used += count;	/* Find unused references */	for ( ref = xen->grant.ref ; count ; ref = ( ( ref + 1 ) & mask ) ) {		/* Sanity check */		assert ( check++ < entries );		/* Skip reserved references */		if ( ref < GNTTAB_NR_RESERVED_ENTRIES )			continue;		/* Skip in-use references */		hdr = xengrant_header ( xen, ref );		if ( readw ( &hdr->flags ) & GTF_type_mask )			continue;		if ( readw ( &hdr->domid ) == DOMID_SELF )			continue;		/* Zero reference */		xengrant_zero ( xen, hdr );		/* Mark reference as in-use.  We leave the flags as		 * empty (to avoid creating a valid grant table entry)		 * and set the domid to DOMID_SELF.		 */		writew ( DOMID_SELF, &hdr->domid );		DBGC2 ( xen, "XENGRANT allocated ref %d/n", ref );		/* Record reference */		refs[--count] = ref;	}	/* Update cursor */	xen->grant.ref = ref;	return 0;}

/** * Transfer bits over SPI bit-bashing bus * * @v bus		SPI bus * @v data_out		TX data buffer (or NULL) * @v data_in		RX data buffer (or NULL) * @v len		Length of transfer (in @b bits) * @v endianness	Endianness of this data transfer * * This issues @c len clock cycles on the SPI bus, shifting out data * from the @c data_out buffer to the MOSI line and shifting in data * from the MISO line to the @c data_in buffer.  If @c data_out is * NULL, then the data sent will be all zeroes.  If @c data_in is * NULL, then the incoming data will be discarded. */static void spi_bit_transfer ( struct spi_bit_basher *spibit,			       const void *data_out, void *data_in,			       unsigned int len, int endianness ) {	struct spi_bus *bus = &spibit->bus;	struct bit_basher *basher = &spibit->basher;	unsigned int sclk = ( ( bus->mode & SPI_MODE_CPOL ) ? 1 : 0 );	unsigned int cpha = ( ( bus->mode & SPI_MODE_CPHA ) ? 1 : 0 );	unsigned int bit_offset;	unsigned int byte_offset;	unsigned int byte_mask;	unsigned int bit;	unsigned int step;	DBGC2 ( spibit, "SPIBIT %p transferring %d bits in mode %#x/n",		spibit, len, bus->mode );	for ( step = 0 ; step < ( len * 2 ) ; step++ ) {		/* Calculate byte offset and byte mask */		bit_offset = ( ( endianness == SPI_BIT_BIG_ENDIAN ) ?			       ( len - ( step / 2 ) - 1 ) : ( step / 2 ) );		byte_offset = ( bit_offset / 8 );		byte_mask = ( 1 << ( bit_offset % 8 ) );		/* Shift data in or out */		if ( sclk == cpha ) {			const uint8_t *byte;			/* Shift data out */			if ( data_out ) {				byte = ( data_out + byte_offset );				bit = ( *byte & byte_mask );				DBGCP ( spibit, "SPIBIT %p wrote bit %d/n",					spibit, ( bit ? 1 : 0 ) );			} else {				bit = 0;			}			write_bit ( basher, SPI_BIT_MOSI, bit );		} else {			uint8_t *byte;			/* Shift data in */			bit = read_bit ( basher, SPI_BIT_MISO );			if ( data_in ) {				DBGCP ( spibit, "SPIBIT %p read bit %d/n",					spibit, ( bit ? 1 : 0 ) );				byte = ( data_in + byte_offset );				*byte &= ~byte_mask;				*byte |= ( bit & byte_mask );			}		}		/* Toggle clock line */		spi_bit_delay();		sclk ^= 1;		write_bit ( basher, SPI_BIT_SCLK, sclk );	}}

/** * Change SNP state from "started" to "stopped" * * @v snp		SNP interface * @ret efirc		EFI status code */static EFI_STATUS EFIAPIefi_snp_stop ( EFI_SIMPLE_NETWORK_PROTOCOL *snp ) {	struct efi_snp_device *snpdev =		container_of ( snp, struct efi_snp_device, snp );	DBGC2 ( snpdev, "SNPDEV %p STOP/n", snpdev );	snpdev->mode.State = EfiSimpleNetworkStopped;	return 0;}

/** * Shut down the network device * * @v snp		SNP interface * @ret efirc		EFI status code */static EFI_STATUS EFIAPIefi_snp_shutdown ( EFI_SIMPLE_NETWORK_PROTOCOL *snp ) {	struct efi_snp_device *snpdev =		container_of ( snp, struct efi_snp_device, snp );	DBGC2 ( snpdev, "SNPDEV %p SHUTDOWN/n", snpdev );	netdev_close ( snpdev->netdev );	snpdev->mode.State = EfiSimpleNetworkStarted;	return 0;}

/** * Dump SNP mode information (for debugging) * * @v netdev		Network device */static void snpnet_dump_mode ( struct net_device *netdev ) {    struct snp_nic *snp = netdev_priv ( netdev );    EFI_SIMPLE_NETWORK_MODE *mode = snp->snp->Mode;    size_t mac_len = mode->HwAddressSize;    unsigned int i;    /* Do nothing unless debugging is enabled */    if ( ! DBG_EXTRA )        return;    DBGC2 ( snp, "SNP %s st %d type %d hdr %d pkt %d rxflt %#x/%#x%s "            "nvram %d acc %d mcast %d/%d/n", netdev->name, mode->State,            mode->IfType, mode->MediaHeaderSize, mode->MaxPacketSize,            mode->ReceiveFilterSetting, mode->ReceiveFilterMask,            ( mode->MultipleTxSupported ? " multitx" : "" ),            mode->NvRamSize, mode->NvRamAccessSize,            mode->MCastFilterCount, mode->MaxMCastFilterCount );    DBGC2 ( snp, "SNP %s hw %s", netdev->name,            snpnet_mac_text ( &mode->PermanentAddress, mac_len ) );    DBGC2 ( snp, " addr %s%s",            snpnet_mac_text ( &mode->CurrentAddress, mac_len ),            ( mode->MacAddressChangeable ? "" : "(f)" ) );    DBGC2 ( snp, " bcast %s/n",            snpnet_mac_text ( &mode->BroadcastAddress, mac_len ) );    for ( i = 0 ; i < mode->MCastFilterCount ; i++ ) {        DBGC2 ( snp, "SNP %s mcast %s/n", netdev->name,                snpnet_mac_text ( &mode->MCastFilter[i], mac_len ) );    }    DBGC2 ( snp, "SNP %s media %s/n", netdev->name,            ( mode->MediaPresentSupported ?              ( mode->MediaPresent ? "present" : "not present" ) :              "presence not supported" ) );}

/** * Select/deselect slave * * @v spibit		SPI bit-bashing interface * @v slave		Slave number * @v state		Slave select state * * @c state must be @c SELECT_SLAVE or @c DESELECT_SLAVE. */static void spi_bit_set_slave_select ( struct spi_bit_basher *spibit,				       unsigned int slave,				       unsigned int state ) {	struct bit_basher *basher = &spibit->basher;	state ^= ( spibit->bus.mode & SPI_MODE_SSPOL );	DBGC2 ( spibit, "SPIBIT %p setting slave %d select %s/n",		spibit, slave, ( state ? "high" : "low" ) );	spi_bit_delay();	write_bit ( basher, SPI_BIT_SS ( slave ), state );	spi_bit_delay();}

/** * Refill receive descriptor ring * * @v netdev		Network device */static void myson_refill_rx ( struct net_device *netdev ) {	struct myson_nic *myson = netdev->priv;	struct myson_descriptor *rx;	struct io_buffer *iobuf;	unsigned int rx_idx;	physaddr_t address;	while ( ( myson->rx.prod - myson->rx.cons ) < MYSON_NUM_RX_DESC ) {		/* Allocate I/O buffer */		iobuf = alloc_iob ( MYSON_RX_MAX_LEN );		if ( ! iobuf ) {			/* Wait for next refill */			return;		}		/* Check address is usable by card */		address = virt_to_bus ( iobuf->data );		if ( ! myson_address_ok ( address ) ) {			DBGC ( myson, "MYSON %p cannot support 64-bit RX "			       "buffer address/n", myson );			netdev_rx_err ( netdev, iobuf, -ENOTSUP );			return;		}		/* Get next receive descriptor */		rx_idx = ( myson->rx.prod++ % MYSON_NUM_RX_DESC );		rx = &myson->rx.desc[rx_idx];		/* Populate receive descriptor */		rx->address = cpu_to_le32 ( address );		rx->control =			cpu_to_le32 ( MYSON_RX_CTRL_RBS ( MYSON_RX_MAX_LEN ) );		wmb();		rx->status = cpu_to_le32 ( MYSON_RX_STAT_OWN );		wmb();		/* Record I/O buffer */		assert ( myson->rx_iobuf[rx_idx] == NULL );		myson->rx_iobuf[rx_idx] = iobuf;		/* Notify card that there are descriptors available */		writel ( 0, myson->regs + MYSON_RXPDR );		DBGC2 ( myson, "MYSON %p RX %d is [%llx,%llx)/n", myson,			rx_idx, ( ( unsigned long long ) address ),			( ( unsigned long long ) address + MYSON_RX_MAX_LEN ) );	}}

/** * Initiate SRP login * * @v srp		SRP device */static void srp_login ( struct srp_device *srp ) {	struct io_buffer *iobuf;	struct srp_login_req *login_req;	int rc;	assert ( ! ( srp->state & SRP_STATE_SOCKET_OPEN ) );	/* Open underlying socket */	if ( ( rc = srp->transport->connect ( srp ) ) != 0 ) {		DBGC ( srp, "SRP %p could not open socket: %s/n",		       srp, strerror ( rc ) );		goto err;	}	srp->state |= SRP_STATE_SOCKET_OPEN;	/* Allocate I/O buffer */	iobuf = xfer_alloc_iob ( &srp->socket, sizeof ( *login_req ) );	if ( ! iobuf ) {		rc = -ENOMEM;		goto err;	}	/* Construct login request IU */	login_req = iob_put ( iobuf, sizeof ( *login_req ) );	memset ( login_req, 0, sizeof ( *login_req ) );	login_req->type = SRP_LOGIN_REQ;	login_req->tag.dwords[1] = htonl ( ++srp_tag );	login_req->max_i_t_iu_len = htonl ( SRP_MAX_I_T_IU_LEN );	login_req->required_buffer_formats = SRP_LOGIN_REQ_FMT_DDBD;	memcpy ( &login_req->port_ids, &srp->port_ids,		 sizeof ( login_req->port_ids ) );	DBGC2 ( srp, "SRP %p TX login request tag %08x%08x/n",		srp, ntohl ( login_req->tag.dwords[0] ),		ntohl ( login_req->tag.dwords[1] ) );	DBGC2_HDA ( srp, 0, iobuf->data, iob_len ( iobuf ) );	/* Send login request IU */	if ( ( rc = xfer_deliver_iob ( &srp->socket, iobuf ) ) != 0 ) {		DBGC ( srp, "SRP %p could not send login request: %s/n",		       srp, strerror ( rc ) );		goto err;	}	return; err:	srp_fail ( srp, rc );}

/** * Change SNP state from "started" to "stopped" * * @v snp		SNP interface * @ret efirc		EFI status code */static EFI_STATUS EFIAPIefi_snp_stop ( EFI_SIMPLE_NETWORK_PROTOCOL *snp ) {	struct efi_snp_device *snpdev =		container_of ( snp, struct efi_snp_device, snp );	DBGC2 ( snpdev, "SNPDEV %p STOP/n", snpdev );	/* Fail if net device is currently claimed for use by iPXE */	if ( efi_snp_claimed )		return EFI_NOT_READY;	snpdev->started = 0;	efi_snp_set_state ( snpdev );	return 0;}