diff --git a/cpukit/include/rtems/rtems-fdt.h b/cpukit/include/rtems/rtems-fdt.h index ff8dbcce29..97d0a92dab 100644 --- a/cpukit/include/rtems/rtems-fdt.h +++ b/cpukit/include/rtems/rtems-fdt.h @@ -233,21 +233,51 @@ bool rtems_fdt_valid_handle( const rtems_fdt_handle *handle ); int rtems_fdt_find_path_offset( rtems_fdt_handle *handle, const char *path ); /** - * Load a device tree blob or DTB file into memory and register it on the chain - * of blobs. + * @brief Loads a device tree blob (DTB) file into memory and registers it on + * the chain of blobs. The DTB may be gzip compressed or binary format. * * @param filename The name of the blob file to load. * @param handle The handle returns the reference to the blob once load. - * @return int If less than 0 it is an error code else it is the blob descriptor. + * @return int If less than 0 an error code else successful. */ int rtems_fdt_load( const char *const filename, rtems_fdt_handle *handle ); +/** + * @brief Loads a raw DTB file int memory and registers it on the + * chain of blobs. + * + * @param filename The name of the raw blob file to load. + * @param handle The handle returns the reference to the blob once load. + * @return int If less than 0 an error code else successful. + * + * @see rtems_fdt_load + */ +int rtems_fdt_load_blob_file_binary( + const char *filename, + rtems_fdt_handle *handle +); + +/** + * @brief Loads a gzip compressed DTB file int memory and registers it on the + * chain of blobs. + * + * @param filename The name of the compressed blob file to load. + * @param handle The handle returns the reference to the blob once load. + * @return int If less than 0 an error code else successful. + * + * @see rtems_fdt_load + */ +int rtems_fdt_load_blob_file_gzip( + const char *filename, + rtems_fdt_handle *handle +); + /** * Register a device tree blob or DTB on to the chain of blobs. * * @param blob_desc A pointer to the blob. * @param handle The handle returns the reference to the blob once load. - * @return int If less than 0 it is an error code else it is the blob descriptor. + * @return int If less than 0 an error code else successful. */ int rtems_fdt_register( const void *blob, rtems_fdt_handle *handle ); diff --git a/cpukit/include/rtems/rtems-fdtimpl.h b/cpukit/include/rtems/rtems-fdtimpl.h index 239404e150..e956a0b0b4 100644 --- a/cpukit/include/rtems/rtems-fdtimpl.h +++ b/cpukit/include/rtems/rtems-fdtimpl.h @@ -47,6 +47,8 @@ #ifndef RTEMS_FDTIMPL_H #define RTEMS_FDTIMPL_H +#include + #include /** @@ -122,4 +124,70 @@ static inline bool rtems_fdt_valid_handle_unprotected( return false; } +static inline int rtems_fdt_deflate( + void *dtb, + size_t dtb_size, + void *cdata, + size_t c_size +) +{ + z_stream stream; + int err; + stream.next_in = (Bytef *) cdata; + stream.avail_in = (uInt) c_size; + stream.next_out = (void *) dtb; + stream.avail_out = (uInt) dtb_size; + stream.zalloc = (alloc_func) 0; + stream.zfree = (free_func) 0; + err = inflateInit( &stream ); + if ( err == Z_OK ) { + err = inflateReset2( &stream, 31 ); + } + if ( err == Z_OK ) { + err = inflate( &stream, Z_FINISH ); + } + if ( ( err == Z_OK ) || ( err == Z_STREAM_END ) ) { + err = inflateEnd( &stream ); + } + if ( ( err != Z_OK ) || ( dtb_size != stream.total_out ) ) { + return -RTEMS_FDT_ERR_READ_FAIL; + } + return 0; +} + +static inline int rtems_fdt_read_file( int fp, void *data, size_t size ) +{ + size_t offset = 0; + int r; + + while ( size ) { + r = read( fp, data + offset, size ); + if ( r < 0 ) { + return r; + } + size -= r; + offset += r; + } + + return offset; +} + +static inline int rtems_fdt_load_file( + const char *filename, + struct stat *stat_buf +) +{ + int bf; + if ( stat( filename, stat_buf ) < 0 ) { + return -RTEMS_FDT_ERR_NOT_FOUND; + } + + bf = open( filename, O_RDONLY ); + if ( bf < 0 ) { + return -RTEMS_FDT_ERR_READ_FAIL; + } + + return bf; +} + #endif diff --git a/cpukit/libmisc/rtems-fdt/rtems-fdt.c b/cpukit/libmisc/rtems-fdt/rtems-fdt.c index 9c5af54050..0506909544 100644 --- a/cpukit/libmisc/rtems-fdt/rtems-fdt.c +++ b/cpukit/libmisc/rtems-fdt/rtems-fdt.c @@ -31,7 +31,6 @@ #include #include -#include #include #include @@ -381,120 +380,36 @@ int rtems_fdt_find_path_offset( rtems_fdt_handle *handle, const char *path ) return -FDT_ERR_NOTFOUND; } -int rtems_fdt_load( const char *filename, rtems_fdt_handle *handle ) +int rtems_fdt_load_blob_file_binary( + const char *filename, + rtems_fdt_handle *handle +) { + struct stat sb; + int bf; void *dtb; size_t bsize; - int bf; ssize_t r; int fe; - struct stat sb; - uint8_t gzip_id[ 2 ]; - uint8_t *cdata; - size_t size; - if ( stat( filename, &sb ) < 0 ) { - return -RTEMS_FDT_ERR_NOT_FOUND; - } - - bf = open( filename, O_RDONLY ); + bf = rtems_fdt_load_file( filename, &sb ); if ( bf < 0 ) { - return -RTEMS_FDT_ERR_READ_FAIL; + return bf; } - /* peek for a gzip header */ - r = read( bf, &gzip_id, sizeof( gzip_id ) ); - if ( r < 0 ) { - close( bf ); - return -RTEMS_FDT_ERR_READ_FAIL; - } - - /* reset file pointer after peek */ - if ( lseek( bf, 0, SEEK_SET ) < 0 ) { - close( bf ); - return -RTEMS_FDT_ERR_READ_FAIL; - } - - /* determine size of the dtb blob */ - if ( ( gzip_id[ 0 ] == 0x1f ) && ( gzip_id[ 1 ] == 0x8b ) ) { - size_t offset; - - cdata = rtems_malloc( sb.st_size ); - if ( !cdata ) { - close( bf ); - return -RTEMS_FDT_ERR_NO_MEMORY; - } - - size = sb.st_size; - offset = 0; - while ( size ) { - r = read( bf, cdata + offset, size ); - if ( r < 0 ) { - free( cdata ); - close( bf ); - return -RTEMS_FDT_ERR_READ_FAIL; - } - size -= r; - offset += r; - } - - offset = sb.st_size - 4; - bsize = - ( ( cdata[ offset + 3 ] << 24 ) | ( cdata[ offset + 2 ] << 16 ) | - ( cdata[ offset + 1 ] << 8 ) | cdata[ offset + 0 ] ); - } else { - cdata = NULL; - bsize = sb.st_size; - } + bsize = sb.st_size; dtb = rtems_malloc( bsize ); if ( !dtb ) { - free( cdata ); close( bf ); return -RTEMS_FDT_ERR_NO_MEMORY; } - /* read the blob */ - if ( ( gzip_id[ 0 ] == 0x1f ) && ( gzip_id[ 1 ] == 0x8b ) ) { - z_stream stream; - int err; - stream.next_in = (Bytef *) cdata; - stream.avail_in = (uInt) sb.st_size; - stream.next_out = (void *) dtb; - stream.avail_out = (uInt) bsize; - stream.zalloc = (alloc_func) 0; - stream.zfree = (free_func) 0; - err = inflateInit( &stream ); - if ( err == Z_OK ) { - err = inflateReset2( &stream, 31 ); - } - if ( err == Z_OK ) { - err = inflate( &stream, Z_FINISH ); - } - if ( ( err == Z_OK ) || ( err == Z_STREAM_END ) ) { - err = inflateEnd( &stream ); - } - if ( ( err != Z_OK ) || ( bsize != stream.total_out ) ) { - free( dtb ); - free( cdata ); - close( bf ); - return -RTEMS_FDT_ERR_READ_FAIL; - } - free( cdata ); - cdata = NULL; - } else { - char *buf = (char *) dtb; - size = bsize; - while ( size ) { - r = read( bf, buf, size ); - if ( r < 0 ) { - free( dtb ); - close( bf ); - return -RTEMS_FDT_ERR_READ_FAIL; - } - size -= r; - buf += r; - } + r = rtems_fdt_read_file( bf, dtb, bsize ); + if ( r < 0 ) { + free( dtb ); + close( bf ); + return -RTEMS_FDT_ERR_READ_FAIL; } fe = rtems_fdt_register( dtb, handle ); @@ -510,6 +425,104 @@ int rtems_fdt_load( const char *filename, rtems_fdt_handle *handle ) return 0; } +int rtems_fdt_load_blob_file_gzip( + const char *filename, + rtems_fdt_handle *handle +) +{ + struct stat sb; + int bf; + uint8_t gzip_id[ 2 ]; + uint8_t *cdata; + void *dtb; + size_t bsize; + ssize_t r; + int fe; + size_t offset; + + bf = rtems_fdt_load_file( filename, &sb ); + if ( bf < 0 ) { + return bf; + } + + /* peek for a gzip header */ + r = read( bf, &gzip_id, sizeof( gzip_id ) ); + if ( r < 0 ) { + close( bf ); + return -RTEMS_FDT_ERR_READ_FAIL; + } + + if ( gzip_id[ 0 ] != 0x1f || gzip_id[ 1 ] != 0x8b ) { + close( bf ); + return -RTEMS_FDT_ERR_READ_FAIL; + } + + /* reset file pointer after peek */ + if ( lseek( bf, 0, SEEK_SET ) < 0 ) { + close( bf ); + return -RTEMS_FDT_ERR_READ_FAIL; + } + + cdata = rtems_malloc( sb.st_size ); + if ( !cdata ) { + close( bf ); + return -RTEMS_FDT_ERR_NO_MEMORY; + } + + r = rtems_fdt_read_file( bf, cdata, sb.st_size ); + if ( r < 0 ) { + free( cdata ); + close( bf ); + return -RTEMS_FDT_ERR_READ_FAIL; + } + + offset = sb.st_size - 4; + bsize = ( ( cdata[ offset + 3 ] << 24 ) | ( cdata[ offset + 2 ] << 16 ) | + ( cdata[ offset + 1 ] << 8 ) | cdata[ offset + 0 ] ); + + dtb = rtems_malloc( bsize ); + if ( !dtb ) { + free( cdata ); + close( bf ); + return -RTEMS_FDT_ERR_NO_MEMORY; + } + + if ( rtems_fdt_deflate( dtb, bsize, cdata, sb.st_size ) < 0 ) { + free( dtb ); + free( cdata ); + close( bf ); + return -RTEMS_FDT_ERR_READ_FAIL; + } + + free( cdata ); + cdata = NULL; + + fe = rtems_fdt_register( dtb, handle ); + if ( fe < 0 ) { + free( dtb ); + close( bf ); + return fe; + } + + handle->blob->name = strdup( filename ); + + close( bf ); + return 0; +} + +int rtems_fdt_load( const char *filename, rtems_fdt_handle *handle ) +{ + int rc; + + rc = rtems_fdt_load_blob_file_gzip( filename, handle ); + + if ( rc < 0 ) { + rc = rtems_fdt_load_blob_file_binary( filename, handle ); + } + + return rc; +} + int rtems_fdt_register( const void *dtb, rtems_fdt_handle *handle ) { rtems_fdt_data *fdt;